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1.0 Introduction 


The Space Station Mechanism Test Bed consists of a hydraulically driven, 
computer controlled six degree of freedom (DOF) motion system with which 
docking, berthing, and other mechanisms can be evaluated. Shown in Figure 1, 
the Test Bed facility simulates the docking of two orbiting vehicles, a 
target and chaser. The chase vehicle docking mechanism is attached to the 
six OOF motion system, while the target's mechanism is fixed to the ceiling 
of the facility. A force and moment sensor is mounted in the ceiling above 
the target docking fixture. Contact forces and- moments due to hardware 
mechanism operation are measured in six degrees of freedom and provided to 
the simulation host computer to enable representation of orbital contact 
dynamics. The old contact dynamics simulation model in use represented a 
restricted case in which one body was considered much larger than the second 
and therefore unaffected by the docking forces and moments. 

The purpose of this report is to describe the development of a genera- 
lized math model to eliminate the restrictive assumptions of the old model. 
This new model represents the relative motion between two rigid orbiting 
vehicles. These vehicles are acted on by forces and moments from vehicle 
contact, attitude control systems, and gravity. ,The resulting model allows 
motion in six degrees of freedom for each body, with no vehicle size limita- 
tion. The new computer simulation is modular to facilitate the addition or 
deletion of various parts of the model. 

This report derives the translational and rotational equations of motion 
for the vehicles in the model. The method used to transform the forces and 
moments from the sensor location to the vehicles' centers of mass is also 
explained. The interface between the dynamics math model and the overall 
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simulation consists of the relative position, velocity, and orientation 
between the vehicles, and the contact forces and moments. This Interface is 
thoroughly discussed. Two math models of docking mechanisms, a simple trans- 
lational spring and the RMS end effector, are presented along with simulation 
results. The translational spring model Is used In an attempt to verify the 
simulation with compensated hardware in the loop results. Finally, recommen- 
dations are made at the end of the report to upgrade and improve the simula- 
tion. 
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2.0 Equations of Motion 

Figure 2 depicts two rigid vehicles undergoing small deviations from a 
nominal circular orbit. The target vehicle is denoted by the number one and 
the chaser is labeled number two. The following coordinate frames and points 
are used by the simulation. Coordinate frame E is an inertial frame fixed at 
the center of the earth. In this frame, the X and Z axes are fixed in the 
equatorial plane with Z pointing at the vernal equinox. Y points to the 
North Pole. The local vertical frame, L, rotates on a circular path of 
radius Ro at a constant angular rate of The Z axis of the local vertical 
frame lies along the position of the local vertical frame with respect to 
the inertial frame. The X axis lies in the plane of the orbit and is tangent 
to the orbit path, while Y is the orbit normal. The orientation of the L 
frame with respect to the E frame is defined through a 2-3-2 Euler angle 
sequence of the following angles. The first two angles, the ascending node 
angle and the angle of inclination, are constant. The third angle, the orbit 
angle, is equal to its initial value plus the product of o\_ and time. The VI 
and V2 frames are fixed at the centers of mass of the target and chase 
vehicles, respectively, and located with respect to the local vertical by the 
position vectors R1 and R2. The D1 and D2 frames are docking port frames 
located at arbitrary positions with respect to the vehicles' centers of mass 
by the vectors RjlVl and R_D2V2 . A sensor coordinate frame, SI, is located 
arbitrarily on the target vehicle with respect to VI by the vector RSIVI . 

The inertial angular velocities of the target and chase vehicles are 
labeled and mj^ and m 2 are the masses of the target and chase 

vehicles, respectively, while II and 12 are the inertia dyadics about the 
centers of mass. 
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The translational equations of motion will now be derived for the target 
vehicle. Those of the chase vehicle will be identical. All forces, except 
gravity, such as vehicle contact and control forces, are included in the vec- 
tor f. Referring to Figure 3, the coordinate frame U lies in the plane of 
the orbit such that the U3 axis is parallel to the Y axis of the local ver- 
tical frame. The position vector Ro is therefore defined by equation (1). 


= RoCOS CJui W + Ro SIN U3ut \J^ 


( 1 ) 


t = time 

The constant angular rate of the local vertical frame, «4_, is given by equa- 
tion (2) 



( 2 ) 


6 = Universal Constant of Gravitation 
M = Mass of the Earth 

Differentiating equation (1) twice with respect to time leads to equation 
(3). 



Ro (Jl (cosULt W+SINUlI 


(3) 


R1 is the position vector of the target vehicle center of mass with respect 
to the local vertical. From Newton's third law, the sum of the forces acting 
on the vehicle must equal the time rata of change of the linear momentum. 


ID 


1 (ro + w] “ R g 


(4) 
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Figure 


The force due to gravity is defined in equation (5) 


If it is assumed that «^Eq|. then using the Binomial theorem and keeping 
only first order terms in Rl, the gravitational force can be written as 

Using equation (2), equation (6) can be written as in (7). 

I"3 = - mi [B° ■‘El) ~ ~Ro^ ) (7) 


Substituting (7) into (4) yields equation (8) 


Ri-u; m(i- 


( 8 ) 


In order to express the components of (8) in the rotating local vertical 
frame, the following notation is now defined. 


R = R + Ulx R 


^ is the time derivative ofj^as seen by an observer fixed in inertial space. 

o 

R is the time derivative of R as seen by an observer in the local vertical 
frame. Since is constant, (9) can be differentiated with respect to time 
to produce (10) . 
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Using equations (8) and (10), and ignoring second order effects, the follow- 


ing equations are generated to govern translational motion of the vehicle. 

XI = ”2 CJlZI fx/mi (11) 

XI = Y 1 +f y/mi (12) 

21 = 2 2 1 'i'fi/mi (13) 

In these equations, XI, Yl, and Z1 define the components of R1 with 

respect to the local vertical frame. This formulation avoids the numerical 

problems of referencing the vehicle position from the center of the earth. 
This is especially useful for contact dynamics simulations which require the 
relative position between the vehicles. 

The rotational equations of motion for the target and chase vehicles are 
the well known Newton - Euler equations shown in (14). 

+ T (14) 


( 0 = Angular Velocity of Vehicle 
I = Inertia Dyadic about Vehicle Center of Mass 
T = Torques Acting about Vehicle Center of Mass 



The torque vector consists of gravity gradient, contact, and attitude 
control system torques. Equation (14) is expressed in a vehicle fixed frame 
so that the inertia dyadic will remain constant. This equation does not 
limit the body axes to align with the principal moment of inertia axes. In 
other words, the inertia matrix does not have to be diagonal. The gravity 
gradient torque acting on the body is expressed in equation (15). 

I OS) 

1 

The Newton - Euler equations are not restricted to small angle rotations 
of tho yGhiclo* 
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3.0 Force/Homent Transformation 

The math model assumes that a force/momemt sensor is located at some 
arbitrary position on the target vehicle which corresponds to its position on 
the ceiling fixture of the 6 DOF facility. However, since the sensor loca- 
tion is not at the center of mass of either vehicle, the measured values must 
be transferred to these points. 

The sensor will measure the resultant forces and torques acting on the 
target vehicle due to hardware contact. If contact occurs at more than one 
point, the force vector measured will be the sum of these contact forces. 
The moment measured will be the resultant torque acting on the target vehicle 
about the sensor location due to the multiple contact forces and moments. 
The force and moment transformation method presented here is independent of 
the number of contact points. 

It is assumed that the inertial forces acting on the moving parts of the 
docking mechanism are negligible. This assumption is valid when the moving 
mass of the docking mechanism is small compared to the mass of the vehicle. 
If this assumption is not violated, then the contact forces and moments are 
equal and opposite between the target and chase vehicles. 

Referring to Figure 2, RSIVI and f^lV2 are the position vectors from the 
target and chase vehicle centers of mass to the sensor, Fs and Ms are now 
defined as the force and moment values from the sensor. Ft, Mt, Fc and jfic 
are defined as the forces and moments acting at the target and chase vehicles' 
centers of mass. The forces and moments from the sensor can now be trans- 
ferred to the vehicles' centers of mass through the following equations. 

Ft = Fs ( 16 ) 
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( 17 ) 


^ = -£s 

= r^s + Rsivj ^ 

Me = -N\j- RSIV2 xFs 
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4.0 Relative Vehicle Motion 


The purpose of the math model is to generate the relative motion between 
the target and chase vehicles when acted on by contact, gravity, and control 
system forces and torques. This data will be used to generate the commands 
for the six OOF motion system. 

As seen in Figure 2, RD201 is the position of the chase docking port 
with respect to the target docking port. It is calculated through equation 
( 20 ). 


RD^i = R2 + RD2V2-R1-RD1V1 


The relative vehicle velocity is defined as the velocity of the chaser 
docking port as seen by an observer on the target docking port. Equation 
(21) defines the relationship between the time derivative of a vector as seen 
by observers fixed in rotating coordinate frames. 

Time derivative of ^ with respect to B frame. 

0 

A = Time derivative of ^ with respect to C frame. 

“C3 = Angular velocity of C frame with respect to B frame. 

Equation (20) is differentiated with respect to time to produce (22) 


RD2DI = R*2 + RD2V2-Rl-RDiVl 

— » ■' -^1 ~ 


13 



Using (21), the following relations are now presented 


RD2D J 


0 

R02D1 = Time derivative of Rj2pi with respect to target docking port, 01, 
frame. 


• O 

R2 = R2 + Ul ^ R2 

R2 = Time derivative of R2 with respect to local vertical frame. 

Rl = RJ.+ (JlX Rl, (25) 

0 

Rl = Time derivative of ^ with respect to local vertical frame. 

The position vectors RD^lVl and RD2y2 are fixed in the vehicle frames. 
Therefore, the time derivatives of these vectors are given as 


R^i = L)i X RDIVI 
B22V2 = X RD2V2 


Substituting (23) through (27) into (22) produces the desired result shown in 
equation (23). 


RD^Dl = RJ. + ((^- (^ij X ^ + 

( Us - Uj) X RD2V2 - w- (0_L- yi)x ^ 


(28) 
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The angular velocity of the chase vehicle with respect to the target 
vehicle is given in equation (29) 


CJ2" Ca)i 


(29) 
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5.0 Simulation Description 


The computer code of the dynamics math model is written entirely in 
FORTRAN 77 and is modular in form. There is a driver routine called by the 
host simulation which controls the math model and acts as the interface to 
the host simulation. 

Figure 4 depicts the program flow as directed by the driver in the dyna- 
mic loop. The first pass through the program initializes the necessary vari- 
ables. The program Input consists of raw force and moment sensor data and 
OMV control system stick commands. The force and moment sensor data is fil- 
tered and transferred to the vehicles' centers of mass. The stick commands 
are used to generate control forces and torques acting on the chase vehicle. 
These forces and moments are used in the equations of motion which are solved 
numerically for the relative position and velocity between the vehicles. 

The following nomenclature used throughout the code is now presented. 
Position and translational velocity vectors begin with the letter R. The 
position vector RABC is defined as the vector from point B to point A 
expressed in C frame coordinates. RABCO is the time derivative of RABC. 
Angular velocity vectors begin with the letters OM. OMABC is defined as the 
angular velocity of A with respect to B in C frame coordinates. The trans- 
formation matrix [AB] transforms vectors from the B frame to A frame as shown 
in equation (30) 

R"'=[ab]r® (30) 

The labels used to describe the vehicles and geometry of Figures 2 and 3 are 
the same as those in the code. 
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DYNAMICS PROGRAM FLOW 



Figure 4 
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The program coimion blocks and subroutines will now be discussed. A 
detailed list of definitions of the global variables is included in Appendix 
1. However, most variable definitions are given in comments throughout the 
code. A program listing of the math model is in Appendix 2. 

There are four files which contain the common blocks and global vari- 
ables used throughout the code. These files are simply included in the 
necessary subroutines of the program. 

The file labeled cmmnbb.f is composed of all variables needed to inter- 
face the dynamics routine with the host simulation. The common block PASS 
consists of the variable OPASS, a program counter used in the OMV control 
system. The variable JPASS counts the number of passes through the dynamic 

loop. Common block INTI contains the variable NNN. NNN may have four dif- 
ferent integer values from -1 to 2. A value of -1 is used for the first ini- 
tialization of the program, while a value of 0 is for program initialization 
between runs. In the dynamic loop, this variable has the value of 1, and a 
value of 2 is used for an end of run print of thruster on times. JPASS and 
NNN are generated in the host simulation to be used in the dynamic math 
model . 

The common block CAOIN is composed of the seven element array KDAT. The 
first 6 elements are X, Y, Z translation and roll, pitch, yaw rotation stick 
commands for the OMV control system. The seventh element is an attitude rata 
hold switch. The common block sensor consists of the ten element array KIN 
and the integer flags NOUSE, IFREEZ, IFLAG, and IPOT. The first 6 elements 
of KIN are X, Y, Z, and roll, yaw, pitch signals from the force and moment 
sensor. Element 7 is the freeze flag and element 8 is used to reset the 
offset forces. Element 9 is the IPOT flag. The dynamic model uses the KIN 
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array to calculate the contact forces and moments, and to generate the four 
integer flags. Common block TIME is composed of the variables T, DT, DT2, 
and TD. T is time. DT and DT2 are the simulation cycle and half cycle 
times. TD is a time delay used in the OMV control system. All of these 
variables are set in the host simulation to be used by the dynamics routine. 
The common block RELATIVE contains the output of the dynamics math model. 
The vector RD2D1D1 is the position vector from the target docking port to the 
chase docking port, in target docking port coordinates. RD2D101D is the 
velocity of the chaser docking port with respect to the target docking port, 
in target docking port coordinates. 0MV2V1V2 is the angular velocity of the 
chase vehicle with respect to the target vehicle, in chase vehicle coor- 
dinates. D1D2 is the transformation matrix from the chaser docking port 
frame to the target docking port frame. 

The common blocks of the file cmmn.f, STAT, EARTH, and TRANS, define the 
size of the state vectors and necessary orbital parameters. In particular, 
OMO is the constant angular velocity of the local vertical frame, and XLE is 
the transformation matrix from the E to L frame. 

The files cmmnl.f and cmmn2.f contain the variables pertaining to the 
target and chase vehicles, respectively. Appendix 1 should be consulted for 
a listing of the variable definitions of these files. The equivalence state- 
ments in these files should be noted. Yl, Y2, YDl, and YD2 are defined as 
the state and state dot vectors for the target and chase vehicles. The state 
vectors are composed of the vehicle angular momentum vectors, quaternions, 
and translational velocity and position vectors. 

The file cmmncon.f contains the variables associated with the NASA OMV 
control system. 
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The file dyn.f is the subroutine DYNAMIC. This subroutine is the driver 
for the math model as shown in Figure 4. This subroutine will be called at 
time T by the host simulation, with no argument list, in order to obtain the 
relative vehicle motion data at time T + DT. The subroutine uses the 3-1 
integration scheme, with the previous state dot values stored in the arrays 
YDOl and YD02. The first pass through DYNAMIC initializes the state and 
state dot vectors, vehicle mass properties and geometry, control system 
parameters, and the force and moment sensor scaling factors. After this 
first pass, the routine begins at the top of the dynamic loop with the force 
and ^moment transformation routine. The state and state dot vectors are then 
calculated and integrated. The vehicle relative motion data contained in the 
common block RELATIVE is generated and returned to the host simulation. 

The subroutines CHASE and TARGET are located in the files chase. f and 
target. f. These routines calculate the state dot vectors for both vehicles. 
The first three elements of the state dot vector are components of the time 
derivative of the angular momentum vector. This vector is generated using 
equation (14) with the chase vehicle having additional torques due to the 
control system. The next four elements of the state dot vector are the 
result of the quaternion differential equations in (31) and (32). 



(32) 


q »■ Vector Part of Quaternion 

q4 = Scalar Part of Quaternion 

oj = Vehicle Angular Velocity Vector 


20 



The quaternions of the state vectors relate the vehicle fixed frames to iner- 
tial space. The next three terms of the state dot vectors are the vehicle 
accelerations with respect to the local vertical frame. These accelerations 
are calculated through equations (11) through (13). The force terms for the 
chase vehicle contain contributions from the OMV control system as well as 
the contact forces. The remaining terms of the state dot vector are the 
vehicle center of mass velocities with respect to the local vertical frame. 

The file cntrlc.f contains the subroutines CNTRLC, QLIMI, and LOGIC 
which comprise the NASA OMV control system. Control system stick commands 
are input to the routines through the array KDAT. The resultant forces and 
moments about the chase vehicle center of mass are output in the V2 frame. 
These forces and moments are labeled FJX, FJY, FJZ, MX, MY, and MZ. 

Subroutine FMTRAN, located in the file fmtran.f, takes in raw data from 
the force and moment sensor through the integer array KIN. This data is then 
scaled and filtered to produce the contact forces and moments acting on the 
target vehicle at the sensor location. From these values, the forces and 
moments acting at the target center of mass are calculated using equations 

(16) and (18), and those acting at the chase vehicle center of mass through 

(17) and (19). 

File intfac.f contains the subroutine INTFAC. This subroutine generates 
the relative vehicle position, velocity, angular velocity, and orientation. 
This subroutine also calculates other transformation matrices used throughout 
the model. The relative vehicle position and velocity, RD20101 and RD201010, 
are generated through equations (20) and (28). The relative angular velocity 
of the chase vehicle with respect to the target vehicle, 0MV2V1V2, is com- 
puted using equation (29) with the resulting vector in the V2 frame. 
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The file libjg.f consists of various math library subroutines. Many of 
these routines handle routine matrix vector operations. There is a matrix 
inverse routine for 3x3 matrices and also routines which generate direction 
cosine matrices from quaternions and vice versa. 

Subroutine STARTT of the file start. f defines the vehicles' mass proper- 
ties and geometry. The orbital parameters and initial transformation matri- 
ces are also input. The effects of gravity can be nullified by setting the 
variable 6MEK to zero. This variable is the product of the universal gravi- 
tational constant and the mass of the earth. This will fix the local ver- 
tical frame in space. The control system parameters are also set at the end 
of this subroutine. 
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6.0 Simulation Verification 


Before integration of the math model with the host simulation, the 
dynamic model must be verified. The entire contact dynamics simulation must 
be validated upon integration before any confidence can be placed in the 
hardware tests. 

The math model was checked out by two means. First, simple forces and 
moments were applied to the vehicles in each axis with the results verified 
by hand. The OMV control system was exercised by commanding translational 
accelerations and angular rates for the different axes and tracking the 
response of the chaser. More complex runs were then made to verify Hill's 
equations, orbital effects, and the Newton - Euler equations. These runs were 
verified through other contact dynamics simulations at Control Dynamics. 

The math model has been successfully transferred to the VAX computer and 
again checked out. The necessary changes have been made to interface it with 
the host simulation. The integrated simulation can now be validated by using 
simple springs as the docking mechanism. After the compensation tests are 
completed, real time results can be generated for various initial condition 
runs. 

An analytic model of the spring system will be used to duplicate the 
real time results. For the case presented here, a linear translational 

spring is attached to the chase vehicle shown in Figure 5. The unit vector 
along the 1 axis of D2, Ul, coincides with the spring. Rrel is the position 
vector of the 01 frame with respect to D2. Contact between the target 
vehicle and the spring is checked by comparing the equilibrium spring length 
to the component of Rrel along Ul. If contact is made, equal and opposite 
forces proportional to the spring compression are applied to the vehicles. 
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The equations governing these contact forces are (33) and (34). 

[ RreJ • Ul] > Le<j , (33) 

(Rrel’W) < Lec| ,^=-KslLe«)-^l-Uj)W 

Leq = Spring Equilibrium Length 
Ks = Spring Constant 

Fc * Contact Force Acting on Chase Vehicle 

Figure 6 depicts the run scenario used to generate the results of 
Figures 7 and 8. The target vehicle is fixed in space with the chase vehicle 
approaching it at a rate of .5 inches per second. The effects of gravity 
have been neglected; therefore, the local vertical frame is fixed in space. 
The chase vehicle has a mass of 326.23 slugs, and the spring constant is 50 
pounds per inch. The simulation cycle time or integration step size is 35 
milliseconds. Since damping has been neglected in the model, the vehicle 
exit velocity should be .5 inches per second. Figure 7 shows the vehicle 
veolcity versus time. When the velocity is zero, the spring compression, and 
force, is a maximum. This is seen in Figure 8. The velocity is presented in 
the local vertical frame, while the forces are plotted in the SI or sensor 
frame. These results would compare favorably with the real time results for 
a perfect system with no time lag or numerical problems. 
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Figure 6 
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CHASE VEHICLE 
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TIME sec 
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TIME se 



7.0 RMS End Effects Math Model 


In section 6, an analytical model of the translational spring was used 
to generate contact forces in place of the actual hardware. These forces 
were fed into the dynamic math model in place of those measured by the sensor 
in order to generate the vehicles' responses. This section describes the 
analytic model of the RMS end effector based on the method of soft con- 
straints. To apply this method, a set of constraint equations describing the 
end effector are derived. Small violations of these constraints are per- 
mitted, but forces proportional to the violation are applied normal to the 
constraining surfaces. 

There are three possible points of contact, shown in Figure 9, for which 
constraint equations are derived. First, there is probe to snare cable con- 
tact. Second, the shoulders at the base of the probe can hit the grooves in 
the collar of the end effector. And third, the shoulders may strike the flat 
of the collar and miss the grooves. It is assumed that there is no contact 
between the probe and the inner walls of the end effector. Damping is also 
neglected. 

Figure 10 depicts two orbiting vehicles with the relative position 
between the two docking ports denoted by Pp. The D1 coordinate frame of the 
target vehicle is fixed to the tip of the probe or grapple fixture. The D2 
frame is at the center of the top of the end effector. 

Figure 11 shows the grapple fixture and the snare cables in an arbitrary 
position. With the grapple fixture inside of the end effector and snared by 
the cables, then the constraint equations governing the probe position are 
(35) and (36). 

d ^ 0 
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POSSIBLE POINTS OF CONTRCT 
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F i gure 
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(36) 


S=o 


Equation (35) simply states that once the probe is snared, it may not leave 
the cylinder of the end effector. Equation (36) implies that the probe 
should remain in the center of the triangle formed by the three snare cables. 
If these equations are violated, then the forces of equations (37) and (38) 
are applied to the probe. 


E= Kd 
E = KS 


K * Snare Cable Stiffness 

Figure 12 depicts the relative positions of the shoulders of the grapple 
fixture and the collar of the end effector. ^ is the position vector from the 
tip of the probe to the outer edge of a shoulder. G is the position of the 
center of a groove with respect to the 02 origin. The width of each groove 
is W. ^is the position vector from a given groove to a given shoulder. ^ 
is calculated from equation (39) for each shoulder and groove combination. 

^ " S” It " £ 

The constraint equation governing the shoulder position is given by equation 
(40) 
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£ • Ha 2 O 


(40) 


This equation simply states that the shoulder may not pass through the col- 
lar. However, if equation (40) is violated, then there is either shoulder to 
groove or shoulder to collar contact. If there is shoulder to groove con- 
tact, then the component of ^ along n^ will be less than half the groove 
width. For shoulder to groove contact, the force of equation (41) is applied 
to the shoulder. 


K * Collar Material Stiffness 

The collar stiffness is generally of the order of ten thousand pounds per 

foot. If the component of e along nj is larger than the groove half width, 

^ / — ' 

then there is shoulder to collar contact. The shoulder force is given by 
equation (42). 

l£* ni| , Fs=Kl£*nJn3 

This force is applied to the shoulder in the direction normal to the collar. 

The analytic contact force model of the RMS end effector has been imple- 
mented into the simulation and exercised. In order to use this model to verify 
real time results of tests using the end effector, the carriage motor speed 
and material stiffness should be investigated to learn more accurate values. 
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8.0 Conclusions and Recommendations 


In fulfillment of the contract objective, a generalized math model has 
been developed to represent the relative motion between two rigid six degree 
of freedom orbiting vehicles. The model has no restrictions on vehicle size 
and is modular in form. The NASA OMV control system has been integrated into 
the model. The dynamics math model is coded in FORTRAN 77 and is currently 
working on the VAX 11-750 of the docking facility. The interface has been 
defined to merge the math model with the host simulation and verify with 
actual real time tests. 

In the future, the math model will be examined in an effort to reduce 
the simulation cycle time. The math model will be modified to streamline the 
leg length calculations. Control Dynamics will also support the three point 
contact study through structural analysis and continued improvement of the 
math model . 
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APPENDIX 1 

VARIABLE DEFINITIONS 


AE 

AIVl 

AIVII 

AIV2 

AIV2I 

DT 

DT2 

D1D2 

DIL 

DlVl 

D2L 

D2V2 

FICL 

FlCSl 

F2CL 

F2CS1 

FJX, FJY, FJZ 

HTVl 

HTV2 

I FLAG 

IFREEZ 

JPASS 

JPOT 

KDAT 

KIN 


Transformation from E Frame to Orbit Plane 

Inertia of Target Vehicle about Center of Mass in VI frame 

Inverse of AIVl 

Inertia of Chase Vehicle about Center of Mass in V2 
Coordinates 

Inverse of AIV2 

Cycle Time 

Half Cycle Time 

Transformation from D2 to D1 Coordinates 

Transformation from L to D1 Coordinates 

Transformation from VI to D1 Coordinates 

Transformation from L to 02 Coordinates 

Transformation from V2 to 02 Coordinates 

Contact Force Acting on Target Vehicle in L Coordinates 

Contact Force from Sensor in SI Coordinates 

Contact Force Acting on Chase Vehicle in L Coordinates 

Contact Force Acting on Chase Vehicle in SI Coordinates 

OMV Control System Forces in V2 Frame 

Angular Momentum of Target Vehicle 

Angular Momentum of Chase Vehicle 

Flag Used in Sensor Offset Forces 

Freeze Flag Set by KIN 

Counter of Dynamic Loop Passes 

Flag Set by KIN 

Array of Joystick Commands for OMV Control System 
Array of Force and Moment Sensor Values and Flags 


MASSl 


Mass of Target Vehicle 
MASS2 - Mass of Chase Vehicle 

MX, MY, MZ - OMV Control System Torques about Chase Vehicle Center of 

Mass in V2 Coordinates 

NNN - Initialization Variable 

NST - Dimension of State Vector 

OMO - Angular Velocity of L Frame 

OMOSQ - OMO Squared 

OMVl - Angular Velocity of Target Vehicle in VI Coordinates 

0MV2 - Angular Velocity of Chase Vehicle in V2 Coordinates 

0MV2V1V2 - Angular Velocity of Chaser with Respect to Target in V2 

Coordinates 

QVIE - Target Vehicle Quaternions Relating VI and E Frames 

QV2E - Chase Vehicle Quaternions Relating V2 and E Frames 

ROIVIVI - Position Vector of Target Docking Port with Respect to VI 

in VI Coordinates 

RD2D1D1 - Position Vector of Chaser Docking Port with Respect to Target 
Docking Port in D1 coordinates 

RD2D1D1D - Time Derivative of RD2D1D1 

RD2V2V2 - Position Vector of Chase Docking Port with Respect to V2 in 
V2 Coordinates 

RNU2 - Current Orbit Angle 

RNU20 - Initial Orbit Angle 

RSIVIVI - Position Vector of Sensor with Respect to VI in VI 

Coordinates 

RSIVISI - Position Vector of Sensor with Respect to VI in SI 

Coordinates 

RS1V2S1 - Position Vector of Sensor with Respect to V2 in SI 

Coordinates 

RIL - Position Vector Target Vehicle with Respect to L Frame 

RILD - Velocity Vector of Target Vehicle with Respect to L Frame 



R2L 

R2LD 

SIL 

SlVl 

S1V2 

T 

TD 

TlCGVl 

TlCGSl 

TlCSl 

TIG 

T2CGS1 

T2CGV2 

T2G 

VIL 

VIE 

V2E 

V2L 

XLE 

Y1 

YDl 

YDOl 

Y2 

YD2 

YD02 


Position Vector of Chase Vehicle with Respect to L Frame 
Velocity Vector of Chase Vehicle with Respect to L Frame 
Transformation from L to SI Coordinates 
Transformation from VI to SI Coordinates 
Transformation from V2 to SI Coordinates 
Time 

OMV Control System Time Delay 

Contact Torque Acting at Target Center of Mass in VI 
Coordinates 

Contact Torque Acting at Target Center of Mass in SI 
Coordinates 

Contact Torque at Sensor in SI Coordinates 

Target Vehicle Gravity Gradient Torque in VI Coordinates 

Contact Torque Acting about Chase Vehicle Center of Mass in 
SI Coordinates 

Contact Torque Acting about Chase Vehicle Center of Mass in 
V2 Coordinates 

Gravity Gradient Torque Acting on Chase Vehicle in V2 
Coordinates 

Transformation from L to VI Coordinates 
Transformation from E to VI Coordinates 
Transformation from E to V2 Coordinates 
Transformation from L to V2 Coordinates 
Transformation from E to L coordinates 
Target Vehicle State Vehicle 
Target Vehicle State DOT Vector 
Previous Value of YDl 
Chase Vehicle State Vector 
Chase Vehicle State Dot Vector 
Previous Value of YD2 


APPENDIX 2 


PROGRAM LISTING 


c 

C THIS FILE CONTAINS THE NECESSARY COMMON BLOCKS TO INTERFACE 

C THE NASA DOCKING SIMULATION WITH THE CONTROL DYNAMICS DOCKING 

C DYNAMICS SUBROUTINE 

C 
C 

INTEGER*2 KIN(IO) ,KDAT(7) 

I NTEGER I FREEZ , NOUSE , I POT , IFLAG , JPASS , NNN 
C 

REAL T,DT,DT2,TD 

REAL RD2D1D1(3) ,RD2D1D1D(3) ,0MV2V1V2(3) ,D1D2(3,3) 

C 

C 

COMMON /PASS/ JPASS 
COMMON /INTI/ NNN 
COMMON /CAD IN/ KDAT 

COMMON /SENSOR/ KIN, NOUSE, IFREEZ,IFLAG,IPOT 
COMMON /TIME/ T,DT,DT2,TD 

COMMON /RELATIVE/ RD2D1D1 ,R02D1D1D,0MV2V1V2,D1D2 
C 

r 

C T IS TIME 

C DT IS THE CYCLE TIME 

C DT2 IS HALF OF THE CYCLE TIME 

C TD IS TIME DELAY FOR THE OMV CONTROL SYSTEM 

C 

C JPASS IS A PROGRAM PASS COUNTER FOR THE OMV CONTROL SYSTEM 

C 

C NNN IS INITIALIZATION VARIABLE 

C 

C KDAT IS A 7 ELEMENT VECTOR FROM THE JOY STICK OF THE OMV 

C CONTROL SYSTEM. THE FIRST 6 ELEMENTS ARE X,Y,Z TRANSLATION 

C AND ROLL, PITCH, AND YAW. ELEMENT 7 IS FOR THE ATTITUDE RATE 

C HOLD SWITCH. 

C 

C KIN IS A 10 ELEMENT VECTOR FROM THE FORCE/MOMENT SENSOR. 

C THE FIRST 6 ELEMENTS ARE X,Y,Z, AND ROLL, YAW, PITCH IN 

C SENSOR COORDINATES. ELEMENT 8 IS USED TO RESET THE OFFSET 

C FORCES. THE FREEZE FLAG IS SET EQUAL TO ELEMENT 7. 

C IPOT IS SET EQUAL TO ELEMENT 9. 

C 

C RD2D1D1 IS THE POSITION VECTOR FROM THE TARGET DOCKING 

C PORT TO THE CHASE DOCKING PORT IN TARGET DOCKING PORT 

C COORDINATES. 

C 

C RD2D1D1D IS THE VELOCITY VECTOR OF THE CHASE DOCKINIG PORT 

C WITH RESPECT TO THE TARGET DOCKING PORT IN TARGET DOCKING 

C PORT COORDINATES. 

C 

C OMV2V1V2 IS THE ANGULAR VELOCITY VECTOR OF THE CHASE VEHICLE 

C WITH RESPECT TO THE TARGET VEHICLE IN CHASE VEHCILE COORDINATES 

C 


o o o 


D1D2 IS THE TRANSFORMATION MATRIX FROM THE CHASE DOCKING PORT 
TO THE TARGET DOCKING PORT. 



o o o o o o 


THIS FILE CONTAINS THE COMMON BLOCKS FOR THE ORBITAL PARAMETERS 
OF THE EQUATIONS OF MOTION AND ALSO THE SIZE OF THE PLANT 
STATE VECTORS. 


REAL OMOSO,OMO,RNU2,RNU20 
REAL XLE(3,3),AE(3,3) 

INTEGER NST 
C 
C 

COMMON /STAT/ NST 

COMMON /EARTH/ RNU20,RNU2,OMOSQ,OMO 
COMMON /TRANS/ AE,XLE 


ooo o r> o oooo 


THIS FILE CONTAINS THE COMMON BLOCKS AND DIMENSIONS 
FOR THE DATA PERTAINING TO BODY 1 OR THE TARGET VEHICLE. 

REAL Y1(13),YD1(13),AIV1{3,3),AIV1I(3,3) 

REAL 0MV1(3),S1V1(3,3),RS1V1V1(3),R1LD(3),R1L(3) 

REAL HTV1(3),0V1E(4),MASS1,V1L(3,3),YD01(13) 

REAL S1L(3,3),V1E(3,3),RS1V1S1(3) 

REAL F1CSK3) ,T1CGS1(3) ,T1G(3) ,T1CS1(3) 

REAL T1CGV1(3),F1CL(3) 

REAL RD1V1V1(3),01V1(3,3),D1L(3,3) 


EQUIVALENCE (HTVl(l) ,Y1(1) ) , 

1 (QV1E(1),Y1(4)), 

2 (R1LD(1),Y1(8)), 

3 {R1L(1),Y1(11)) 


COMMON /STATEl/ Y1,YD1,YD01 

COMMON /MASl/ MASSl ,AIV1 ,AIV1I ,RS1V1V1 ,RS1V1S1 .RDIVIVI 

COMMON /BODYl/ OMVIJIG 

COMMON /TRANSl/ V1L,S1V1,V1E,S1L,D1V1,D1L 

COMMON /CONTCl/ T1CGV1,F1CL,F1CS1,T1CGS1,T1CS1 



ooo ooo oooo 


THIS FILE CONTAINS THE COMMON BLOCKS AND DIMENSIONS 
FOR THE DATA PERTAINING TO BODY 2 OR THE CHASE VEHICLE. 

REAL Y2(13) ,YD2(13) ,AIV2(3,3) ,AIV2I(3,3) 

REAL 0MV2(3),D2V2(3,3),RD2V2V2(3),R2LD(3),R2L(3) 

REAL HTV2(3) ,QV2E(4) ,MASS2,V2L(3,3) ,YD02(13) 

REAL V2E(3,3),RS1V2S1(3) 

REAL F2CS1(3),T2CGS1(3),T26(3) 

REAL T2CGV2(3) ,F2CL(3) ,S1V2(3,3) ,D2L(3,3) 


EQUIVALENCE (HTV2(1) ,Y2(1) ) , 

1 (QV2E(1),Y2(4)), 

2 (R2LD(1),Y2(8)). 

3 (R2L(1),Y2(11)) 


COMMON /STATE2/ Y2.YD2,YD02 

COMMON /MAS2/ MASS2,AIV2,AIV2I ,RD2V2V2,RS1V2S1 

COMMON /B0DY2/ 0MV2.T2G 

COMMON /TRANS2/ V2L,D2V2,V2E,S1V2,D2L 

COMMON /C0NTC2/ F2CS1,T2CGS1,T2C6V2,F2CL 
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THIS FILE CONTAINS THE NECESSARY COMMON BLOCKS FOR THE NASA 
OMV CONTROL SYSTEM. 

INTEGER*4 EAX ,EAY ,EAZ ,EMX ,EMY ,EMZ,NAD ,NDCI ,NDC , lAUTOP , ISTBSL 
I NTEGER*4 MPASS , I SAMP , I JONS( 3) , I J0FS( 3) , I S 
REAL SSIN(7) ,YIN,RTD,DTR,SCX,SCY,SCZ,AFUEL 
REAL P31,P32,P33,P34,P031,PD32,PD33,PD34,P31M1,P32M1,P33M1 
REAL P34M1 ,PD31M1 ,PD32M1 ,PD33M1 ,PD34M1 ,DELAY(80 ,6) 

REAL WVX2A ,WVY2A ,WVZ2A , NOI SEl , N0ISE2 , NO I SQ 
REAL MX,MY,MZ,FJX,FJY,FJZ 
C 
C 

COMMON /ADIN/ SSIN,YIN,NDC,NAD,NDCI 
COMMON /DEG/ RTO.DTR 
COMMON /MSB/ ISTBSL 
COMMON /SAMP/ MPASS, I SAMP 

COMMON /LOG/ EAX,EAY,EAZ,EMX,EMY,EMZ,IJONS,IJOFS 

COMMON /MOMV/ AFUEL,WVX2A,WVY2A,WVZ2A 

COMMON /CONFOR/ MX,MY,MZ,FJX,FJY,FJZ 

COMMON /SCALE/ SCX,SCY,SCZ 

COMMON /NOIS/ N0ISE1,N0ISE2,N0ISQ 

COMMON /LAG/ DELAY, IS 

COMMON /ERRORl/ P31,P32,P33,P34,P31M1,P32M1,P33M1,P34M1 
COMMON /ERROR2/ PD31 ,PD32,PD33,PD34,PD31M1 ,PD32M1 ,PD33M1 ,PD34M1 
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PHYSICAL SYSTEM (PLANT) SUBROUTINE FOR CHASE VEHICLE 

THE CHASE VEHICLE IS DENOTED BY THE NUMBER 2. 

THIS SUBROUTINE CALCULATES THE TIME DERIVATIVE OF THE 
CHASE VEHICLE STATE VECTOR. IT USES NEWTON-EULER AND 
HILL'S EQUATIONS. 

THE INPUT TORQUES ARE FROM THE OMV CONTROL SYSTEM, VEHICLE 
CONTACT, AND GRAVITY GRADIENT. 

THE INPUT FORCES ARE FROM THE CONTROL SYSTEM AND VEHICLE 
CONTACT 


SUBROUTINE CHASE 


INCLUDE 'cmmn.f 
INCLUDE 'ctnmnZ.f' 

TWri imp 'rmmnrnr'.f 


REAL FXL,FYL,FZL,VTEM(3) 
INTEGER 1,0 


EXTRACT THE PHYSICAL PARAMETERS OUT OF THE STATE VECTOR Y. 


ASSUME THAT THE STATE VECTOR IS CONSTRUCTED AS FOLLOWS: 
Y={HTV2,0V2E,R2LD,R2L) 

HTV2 IS THE ANGULAR MOMENTUM VECTOR OF THE CHASE VEHICLE. 

0V2E IS THE QUATERNION FOR THE TRANSFORMATION FROM THE 

INERTIAL, E, FRAME TO THE CHASE VEHICLE FIXED FRAME. 
R2LD IS THE VELOCITY OF THE CHASE VEHICLE CENTER OF MASS 

WITH RESPECT TO THE L FRAME AND IN L FRAME COORDINATES. 
R2L IS THE POSITON OF THE CHASE VEHICLE CENTER OF MASS 

WITH RESPECT TO THE L FRAME AND IN L FRAME COORDINATES. 


DO 100 1=1, NST 
YD02(I)=YD2(I) 
CONTINUE 


COMPUTE THE TOTAL GRAVITY GRADIENT TORQUES 


DO 1500 1=1,3 
VTEM(I)=0.0 
DO 1500 J=l,3 

VTEM( I )=VTEM( I )+AIV2( I ,J)*V2L( J ,3) 

1500 CONTINUE 

T2G(l)=3.O*OMOS0*(V2L(2,3)*VTEM(3)-V2L(3,3)*VTEM{2)) 

T2G(2)=3.O*OM0S0*(V2L(3,3)*VTEM(l)-V2L(l,3)*VTEM(3)) 


oooooo ooooo ooooo oooooo 


T2G{3)=3.0*0M0S0*(V2L(1,3)*VTEM(2)-V2L(2,3)*VTEM(1)) 


CALCULATE THE DERIVATIVE OF THE ANGULAR MOMENTUM VECTOR 
USING NEWTON-EULER EQUATION. 


YD2( 1 ) =MX+T2G( 1 )+T2CGV2( 1 ) -0MV2( 2)*HTV2{ 3)+0MV2( 3 )*HTV2( 2) 
YD2 ( 2 ) =MY+T2G( 2 ) +T2CGV2 ( 2 ) -0MV2 ( 3 ) *HTV2 ( 1 ) +0MV2 { 1 ) *HTV2 ( 3 ) 
YD2(3)=MZ+T2G(3)+T2CGV2(3)-0MV2(1)*HTV2(2)+0MV2(2)*HTV2(1) 


CALCULATE THE DERIVATIVE OF THE QUATERNION 


YD2(4)=.5*(0MV2(3)*0V2E(2) 

$ -0MV2(2)*QV2E(3)+0MV2(1)*QV2E(4)) 

YD2(5)=.5*(-0MV2(3)*QV2E(1) 

$ +0MV2(1)*QV2E(3)+0MV2(2)*QV2E(4)) 

YD2(6)=.5*(0MV2(2)*QV2E(1) 

$ -0MV2 ( 1 ) *QV2E ( 2 ) +0MV2 ( 3 ) *QV2E ( 4 ) ) 

YD2(7)=-.5*(0MV2(1)*0V2E(1) 

$ +0MV2(2)*QV2E(2)+0MV2(3)*0V2E(3)) 


TRANSFORM FORCES OF CONTROL SYSTEM FROM V2 FRAME TO L FRAME 


FXL=FJX*V2L(1,1)+FJY*V2L(2,1)+FJZ*V2L(3,1) 

FYL=FJX*V2L(1,2)+FJY*V2L(2,2)+FJZ*V2L(3,2) 

FZL=FJX*V2L(1,3)+FJY*V2L(2,3)+FJZ*V2L(3,3) 


CALCULATE THE ACCELERATION OF THE VEHICLE CENTER OF MASS 
WITH RESPECT TO THE L FRAME IN THE L FRAME COORDINATES. 


YD2(8)=(FXL+F2CL(1))/MASS2-2.*0M0*R2LD(3) 

YD2(9)=(FYL+F2CL(2))/MASS2-0M0SQ*R2L(2) 

YD2(10)=(FZL+F2CL(3))/MASS2+3.*OMOSQ*R2L(3)+2.*OMO*R2LD(1) 

YD2(11)=Y2(8) 

YD2(12)=Y2(9) 

YD2(13)=Y2(10) 


RETURN 

END 
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SUBROUTINE CNTRLC IS THE NASA RCS THRUSTER CONTROL SYSTEM FOR 
THE OMV 

THE INPUT TO THIS ROUTINE REOUIRES THE CHASE VEHICLE INERTIAL 
ANGULAR RATES IN THE BODY FIXED FRAME AND THE ANGULAR VELOCITY 
OF THE LOCAL VERTICAL FRAME. 

IT ALSO REQUIRES THE INPUT FROM THE JOY STICK IN THE KDAT 
VECTOR. 

THE ROUTINE RETURNS THE CONTROL FORCES AND TORQUES IN THE BODY 
FIXED FRAME. 

SUBROUTINE CNTRLC 

INCLUDE 'cmrnn.f 
INCLUDE 'cmmncon.f 
INCLUDE 'cmmn2.f 
INCLUDE 'cmmnbb.f 

INTEGERS IU,I 

REAL XIN(7),XDAT(7),PHIE2X,PHIE2Y,PHIE2Z,EX,EY,EZ 
REAL PD31M2 ,PD32M2 ,PD33M2 , PD34M2 ,DW13 ,DW23 ,DM33 
REAL SW13,SW23,SW33,P3N0RM,R0LIN,PITIN,YAWIN 
REAL ROLL , P ITCH , YAW ,RSQ , YSQ , PSQ ,RSQ1 ,PSQ1 , YSOl 
REAL RS02 , YSQ2 , PSQ2 , RSQ3 , PSQ3 , YSQ3 , EAX I N , EAY I N , EAZ I N 
REAL EAXA ,EAYA , EAZA ,WVX , WV Y , WVZ ,WCVX2 , WCVY2 , WCVZ2 
REAL WVX2,WVY2,WVZ2 
REAL OLIMl 

LOGICAL SW,COMR,COMY,COMP,COM 


SET INERTIAL CHASE VEHICLE ANGULAR RATES FOR CONTROL SYSTEM 

WVX2A=0MV2(1) 

WVY2A=0MV2(2) 

WVZ2A=0MV2(3) 

WVX2=WVX2A-OMO*V2L( 1 ,2) 

WV Y2=WV Y2A-0M0*V2L (2,2) 

WVZ2=WVZ2A-0M0*V2L (3,2) 


KDAT IS THE ANALOG INPUT FROM THE STICK 

DO 1001 1=1, NAD, 1 

XDAT(I)=FLOAT(KDAT(D) 
XIN(I)=XDAT(I)*YIN 
1001 CONTINUE 

ROLIN = XIN(4)*SSIN(4) 

PITIN = XIN(5)*SSIN(5) 

YAWIN = XIN(6)*SSIN(6) 
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***DELAY IS AN ARRAY TO DELAY XIN(I) FROM HAND CONTR.*** 

IF(IS .GE. NDC+DIS = 0 
IS = IS + 1 


C 

lU = IS - NDC 
IF(IU ,LE. 0) lU = IS + 1 
C 

DELAY(IS,4) = ROLIN 
DELAY(IS,5) = PITIN 
0ELAY(IS,6) = YAWIN 
C 

ROLL = DELAY(IU,4) 

PITCH = DELAY(IU,5) 

YAW = DELAY(IU,6) 

C 

IF(ROLL .LT. NOISEl .AND. ROLL .GT. (-N0ISE1) )R0LL=0.0 
IF(PITCH .LT. NOISEl .AND. PITCH .GT. (-NOISED )PITCH=0.0 
IF(YAW .LT. NOISEl .AND. YAW .GT. (-NOISEl) )YAW=0.0 
IF(XIN(7)*SSIN(7) .LT. -.5)G0 TO 36 
SW=. FALSE. 

GO TO 37 

36 SW=.TRUE. 

37 IF(ROLL .GE. NOISEl .OR. ROLL .LE. -N0ISE1)G0 TO 30 
COMR=. FALSE. 

R0LL=0.0 
GO TO 31 

30 COMR=.TRUE. 

31 IF(PITCH .GE. NOISEl .OR. PITCH .LE. -N0ISE1)G0 TO 32 
COMP=. FALSE. 

PITCH=0.0 

GO TO 33 

32 COMP=.TRUE. 

33 IF(YAW .GE. NOISEl .OR. YAW .LE. -N0ISE1)G0 TO 34 
COMY=. FALSE. 

YAW=0.0 
GO TO 35 

34 COMY=.TRUE. 

35 COM=COMR .OR. COMP .OR. COMY 
RSO=ROLL*ABS(ROLL) 

PSO=PITCH*ABS( PITCH) 

YSO=YAW*ABS(YAW) 

IF(SW .AND. COM) GO TO 38 

RS01=RSQ+RS02 

PS01=PS0+PS02 

YS01=YS0+YS02 

RS03=SCX*RSQ1 

PS03=SCY*PS01 
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YSQ3=SCZ*YSQ1 


GO TO 39 

38 RSQ2=RSQ1 
PSQ2=PSQ1 
YSQ2=YSQ1 

39 WCVX2=RS03*DTR 
WCVY2=PS03*OTR 
WCVZ2=YS03*DTR 

DEFINE THE ERROR ANGLES FROM THE QUATERNION P3 

IF(IAUTOP .EQ. 0) GO TO 4 
P31 = 0.0 

P32 = 0.0 

P33 = 0.0 

P34 = 1.0 

PD31M1 = 0.0 
PD32M1 = 0.0 
PD33M1 = 0.0 
PD34M1 = 0.0 
PD31 = 0.0 
PD32 * 0.0 
PD33 = 0.0 
PD34 = 0.0 
GO TO 15 
4 CONTINUE 
PD31M2=PD31M1 
PD32M2=PD32M1 
PD33M2=PD33M1 
PD34M2=PD34M1 
PD31M1=PD31 
PD32M1=PD32 
PD33M1=PD33 
PD34M1=PD34 

DW13=0.5*(WVX2 - WCVX2) 

SW13=0.5*(WVX2 + WCVX2) 

DW23=0.5*(WVY2 - WCVY2) 

SW23=0.5*(WVY2 + WCVY2) 

DW33=0.5*(WVZ2 - WCVZ2) 

SW33=0.5*(WVZ2 + WCVZ2) 

PD31=P34*DW13 - P33*SW23 + P32*SW33 

PD32=P33*SW13 +P34*DW23 - P31*SW33 

PD33= -P32*SW13 + P31*SW23 + P34*DW33 

PD34= -P31*DW13 - P32*DW23 - P33*DW33 

P31M1=P31 

P32M1=P32 

P33M1=P33 

P34M1=P34 

P31=P31M1 + 0T2*(3.0*PD31M1 - PD31M2) 

P32=P32M1 +DT2*(3.0*PD32M1 - PD32M2) 

P33=P33M1 + DT2*(3.0*PD33M1 - PD33M2) 

P34=P34M1 + DT2*(3.0*PD34M1 - PD34M2) 


P3N0RM=SQRT( P31**2+P32**2+P33**2+P34**2 ) 

P31 =P31/P3N0RM 

P32 =P32/P3N0RM 

P33 =P33/P3N0RM 

P34 =P34/P3N0RM 

15 CONTINUE 

PHIE2X= -2.0*RTD*P31 
PHIE2Y= -2.0*RTD*P32 
PHIE2Z= -2.0*RTD*P33 
PHIE2X=0LIM1(-5.,PHIE2X,5.) 
PHIE2Y=0LIM1(-5..PHIE2Y,5.) 
PHIE2Z=QLIM1(-5.,PHIE2Z,5.) 

C 

EAXIN =-XIN( 1)*SSIN( 1) 

EAYIN =-XIN( 2)*SSIN( 2) 

EAZIN =+XIN( 3)*SSIN( 3) 

C 

DELAY(IS,1) = EAXIN 
DELAY(IS,2) = EAYIN 
DELAY(IS,3) = EAZIN 
C 

EAXA = DELAY(IU,1) 

EAYA = DELAY(IU,2) 

EAZA = DELAY(IU,3) 

C 

WVX=RTD*WVX2A 

WVY=RTD*WVY2A 

WVZ=RTD*WVZ2A 

EX=67.5*PHIE2X-169.5*(WVX-RS03) 

EY=45. *PHIE2Y-110.625*(WVY-PSQ3) 

EZ=45. *PHIE2Z-110.625*(WVZ-YSQ3) 

C 

MP ASS=MOD ( J PASS , I SAMP ) 

C ACCEPT HAND CONTROLLER INPUTS ONLY EVERY ISAMP TIMES 
IF(MPASS .NE. 0) GO TO 16 
EAX=0 
EAY=0 
EAZ=0 
EMX=0 
EMY=0 
EMZ=0 

IF(EAXA .GT. N0ISE2) EAX=1 
IF(EAXA .LT. -N0ISE2) EAX=-1 
IF(EAYA .GT. N0ISE2) EAY=1 
IF(EAYA .LT. -N0ISE2) EAY=-1 
IF(EAZA .GT. N0ISE2) EAZ=1 
IF(EAZA .LT. -N0ISE2) EAZ=-1 
IFdAUTOP .NE. 0) GO TO 160 
IF(EX .GE. 17.) EMX=1 
IF(EX .LE. -17.) EMX=-1 
IF(EY .GE. 22.) EMY=1 
IF(EY .LE. -22.) EMY=-1 
IF(EZ .GE. 22.) EMZ=1 
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c 


c 

c 

c 

c 


IF(EZ .LE. -22.) EMZ=-1 
GO TO 16 

EXECUTE AT 160 IF lAUTOP .NE. 0 
160 IF(RS01 .6T. NOISO)EMX=1 
IF(RSQ1 .LT. -N0IS0)EMX=-1 
IF(PS01 .6T. N0ISQ)EMY=1 
IF(PS01 .LT. -N0IS0)EMY=-1 
IF(YS01 .GT. NOISO)EMZ=1 
IF(YSQ1 .LT. -N0IS0)EMZ=-1 
16 CONTINUE 
CALL LOGIC 
RETURN 
END 

FUNCTION 0LIM1(BL,V,TL) 

THIS FUNCTION LIMITS V TO BL OR TL 

BL IS BOTTOM LIMIT 

TL IS TOP LIMIT 

REAL BL,TL,V,0LIM1 

QLIM1=V 

IF(V .LT. BL)0LIM1=BL 
IF(V .GT. TL)QLIM1=TL 
RETURN 
END 

SUBROUTINE LOGIC 
SAVE 

INCLUDE 'cmmncon.f 
INCLUDE 'cmmnbb.f 

RFAI AVI AY1 AY? RO FTR 

R EAL AZl AZ2 1 CX , C Y , CZ , FM , FTRX , RT IME , ST I ME , SSUMRT , SUMRT 
REAL SUMF ,SUMFX ,SUMRTL ,WXDB ,WYDB ,WZDB 

INTEGER ID,IDS,IFLAG1,II,IFX,IFY,IFZ,IR0TRY,IR0TRZ,ISIDEA 
I NTEGER ISDA, ISDB , I SIDEB , JETN , JETC , JET , JTIME ,M ,NCOUNT 
INTEGER NFT,NFP,I2,IJET0N,IJET0F,L,I,J,K 
DIMENSION JETN(24),JETC(24) 

DIMENSION FM(24,6),JET(24),IJET0N(3),IJET0F(3) 

DIMENSION ISI0EA(12),ISI0EB(12) 

DIMENSION 12(3) 

DIMENSION JTIME(24),RTIME(24),ID(24),STIME(24) 

DIMENSION SUMRT(24),SUMRTL(24),IDS(24) 

DIMENSION SSUMRT(24) 

DATA 

*IJETON/0,0,0/, 

*IJET0F/0.0,0/ 

DATA ISIDEA/1,2,3,7,8,9,13,14,15,19,20,21/ 

DATA ISIDEB/4,5,6,10,11,12,16,17,18,22,23,24/ 

DATA ISDA,ISDB/0,0/ 

DATA WXDB,WYDB,WZDB/.l,.l,.l/ 

DATA NFT/0/ 
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DATA NFP/0/ 

C CONSF = FTR/SPECIFIC IMPULSE(SET AT 220 SEC)=0.031818 
IF(NNN)18,18,30 
18 FTR=10.0 

FTRX=10.0 
AX1=16. 523/12. 

AY1=28. 529/12. 

AY2=35.096/12. 

AZ1=66.402/12. 

AZ2=72.97/12. 

DO 20 1=1,24 

DO 20 J=l,6 
20 FM(I,J)=0.0 
DO 431 I = 1,24 
JETN(I) = 0 
JETC(I) = 0 
SUMRT(I)=0.0 
431 CONTINUE 
12(1) = 0 
12(2) = 0 
12(3) = 0 
NCOUNT = 0 

FM(I,J) ARE COEFFICIENTS FOR FORCES AND MOMENTS 
WHERE I IS THRUSTER NO. 

THE SIGN IS FROM BOTH FORCE AND ARM. 

X FORCES 
FM(1,1)=1.0 
FM(2,1)=1.0 
FM(3,1)=1.0 
FM(4,1)=1.0 
C -X FORCES 

FM(5,1)=-1.0 
FM(6,1)=-1.0 
FM(7,1)=-1.0 
FM(8,1)=-1.0 
C Y FORCES 
FM(9,2)=1.0 
FM(10,2)=1.0 
FM(11,2)=1.0 
FM(12,2)=1.0 
C -Y FORCES 

FM(13,2)=-1.0 
FM(14,2)=-1.0 
FM(15,2)=-1.0 
FM(16,2)=-1.0 
C Z FORCES 

FM(17,3)=1.0 
FM(18,3)=1.0 
FM(19,3)=1.0 
FM(20,3)=1.0 
C -Z FORCES 

FM(21,3)=-1.0 

FM(22,3)=-1.0 



FM(23,3)=-1.0 
FM(24,3)=-1.0 
C X MOMENT(ROLL) 

FM(9,4)=AZ1 
FM(10,4)=AZ1 
FM(15,4)»AZ1 
FM(16,4)=AZ1 
FM(18,4)*AY1 
FM(19,4)=AY1 
FM(22,4)=AY1 
FM(23.4)=AY1 
C -X MOMENT(-ROLL) 

FM(11,4)=-AZ1 
FM(12,4)=-AZ1 
FM(13,4)=-AZ1 
FM(14,4)=-AZ1 
FM(17,4)=-AY1 
FM(20,4)=-AY1 
FM(21,4)=-AY1 
FM(24,4)=-AY1 
C Y MOMENT( PITCH) 

FM(17,5)=AX1 
FM(18,5)=AX1 
FM(23,5)=AX1 
FM(24,5)=AX1 
C -Y MOMENT( -PITCH) 

FM(19,5)=-AX1 
FM(20,5)=-AX1 
FM(21,5)=-AX1 
FM(22,5)=-AX1 
C Z MOMENT(YAW) 

FM(10,6)=AX1 
FM(11,6)=AX1 
FM(14,6)=AX1 
FM(16,6)=AX1 
C -Z MOMENT(-YAW) 

FM(9,6)=-AX1 

FM(12,6)=-AX1 

FM(13,6)=-AX1 

FM(15,6)=-AX1 

RETURN 

30 CONTINUE 

IF(NNN ,EQ. 2)G0 TO 1050 
FJX = 0. 

FJY=0.0 

FJZ=0.0 

MX=0.0 

MY=0.0 

MZ=0.0 

C FTR =ACOF*(PLBS-AFUEL)+BCOF 
DO 40 1=1,24 
JET(I)=0 
40 CONTINUE 


IF(ISTBSL.NE.O) GO TO 5000 
IF(EAX) 210,230,220 
210 JET( 5) = 1 
JET( 6) * 1 
JET( 7) = 1 
JET(8) = 1 
GO TO 230 
220 JET(l) = 1 
JET(2) * 1 
JET(3) = 1 
JET(4) » 1 
230 CONTINUE 

IROTRY = EMX 
IROTRZ = 0 
IFLAGl = 0 
IF(EMX) 240,300,240 
240 IF(EMZ) 250,270,250 
250 IF(EMY) 260,290,260 
260 IF(EAY) 285,300,285 
270 IF(EAY) 280,300,280 
280 IF(EMY) 300,290,300 
285 IFLAGl = 1 
290 IROTRZ = EMX 
IROTRY = 0 
300 CONTINUE 

IF( IROTRY) 320,305,310 
305 IF( IROTRZ) 340,350,330 
310 JET(9) = 1 
JET(16) = 1 
JET(IO) = 1 
JET(15) = 1 
GO TO 350 
320 JET( 12) = 1 
JET(14) = 1 
JET(13) = 1 
JET(ll) = 1 
GO TO 350 
330 JET( 18) » 1 
JET( 19) = 1 
JET(22) = 1 
JET(23) = 1 
GO TO 350 
340 JET( 17) = 1 
JET(20) = 1 
JET(21) = 1 
JET(24) = 1 
350 CONTINUE 

IF(EMY) 370,380,360 
360 JET(17) = 1 
JET(18) * 1 
JET(23) = 1 
JET{24) = 1 
GO TO 380 


370 JET( 19) = 1 
JET(20) = 1 
JET(21) = 1 
JET(22) = 1 
380 CONTINUE 

IF(EMZ) 400,410,390 
390 JET( 10) » 1 
JET(ll) = 1 
JET(14) = 1 
JET(16) = 1 
GO TO 410 
400 JET( 9) = 1 
JET( 12) = 1 
JET(13) * 1 
JET(15) = 1 
410 CONTINUE 

IF(EAY) 430,440,420 
420 JET(9) = 1 
JET(IO) = 1 
JET(ll) = 1 
JETI12) = 1 
GO TO 440 
430 JET( 13) = 1 
JET( 14) = 1 
JET(15) = 1 
JET(16) = 1 
440 CONTINUE 

IF(EAZ) 460,470,450 
450 JET( 17) = 1 
JET(18) = 1 
JET(19) = 1 
JET(20) = 1 
GO TO 470 
460 JET( 21) = 1 
JET( 22) * 1 
JET(23) = 1 
JET(24) * 1 

470 IF(JET(14)) 475,475,471 

471 IF(JET(9)) 475,475,472 

472 JET(14) = 0 
JET(9) = 0 

475 IF(JET(10)) 480,480,476 

476 IF(JET( 13)) 480,480,477 

477 JET(IO) = 0 
JET( 13) = 0 

480 IF(JET(12)) 485,485,481 

481 IF(JET(16)) 485,485,482 

482 JET(12) = 0 
JET(16) = 0 

485 IF(JET(11)) 490,490,486 

486 IF(JET(15)) 490,490,487 

487 JET(ll) = 0 
JET(15) = 0 


490 IF(JET(17)) 495,495,491 

491 IF(JET(22)) 495,495,492 

492 JET( 17) = 0 
JET( 22) = 0 

495 IF(JET(18)) 500,500,496 

496 IF(JET(21)) 500,500,497 

497 JET(18) » 0 
JET(21) = 0 

500 IF(JET(19)) 505,505,501 

501 IF(JET(24)) 505,505,502 

502 JET(19) = 0 
JET(24) * 0 

505 IF(JET(20)) 510,510,506 

506 IF(JET(23)) 510,510,507 

507 JET(20) = 0 
JET(23) » 0 

510 CONTINUE 
IF(IFLAGl) 560,560,511 

511 IF(EAZ) 515,560,515 

515 IF(JET(17)) 520,520,516 

516 JET(24) = 1 
JET(20) = 1 
JET(18) = 1 
60 TO 560 

520 IF(JET(18)) 525,525,521 

521 JET(17) = 1 
JET(19) = 1 
JET(23) = 1 
GO TO 560 

525 IF(JET(19)) 530,530,526 

526 JET(18) = 1 
JET(20) = 1 
JET(22) = 1 
60 TO 560 

530 IF(JET(20)) 535,535,531 

531 JET(17) = 1 
JET(19) = 1 
JET( 21) = 1 
60 TO 560 

535 IF(JET(21)) 540,540,536 

536 JET(20) = 1 
JET(22) = 1 
JET(24) = 1 
60 TO 560 

540 IF(JET(22)) 545,545,541 

541 JET(19) = 1 
JET(21) = 1 
JET(23) = 1 
GO TO 560 

545 IF(JET{23)) 550,550,546 

546 JET(24) = 1 
JET(22) = 1 
JET(18)= 1 


60 TO 560 

550 IF(JET(24)) 560,560,551 

551 JET(17) = 1 
JET(21) = 1 
0ET(23) = 1 
60 TO 560 

560 CONTINUE 

IF(I2(D) 600,600,565 
565 IF(IROTRY) 590,570,590 
570 IF(IROTRZ) 575,600,575 
575 IF(EAZ) 600,580,600 

580 IF(EMY) 600,581,600 

581 JET(18) = 0 
JET(20) = 0 
JET(21) » 0 
JET(23) = 0 
60 TO 600 

590 IF(EAY) 600,595,600 

595 IF(EMZ) 600,596,600 

596 JET(ll) = 0 
JET(IO) = 0 
JET(13) = 0 
JET(15) = 0 

600 CONTINUE 

IF(I2(2)) 630,630,605 
605 IF(EMY) 610,630,610 
610 IF(EAZ) 630,615,630 
615 IF(IROTRZ) 630,620,630 
620 JET(17) = 0 
JET(20) = 0 
JET(22) = 0 
JET(23) = 0 
630 CONTINUE 

IF(I2(3)) 700,700,635 
635 IF(EMZ) 640,700,640 
640 IF(EAY) 700,645,700 
645 IF(IROTRY) 700,650,700 
650 JET(ll) = 0 
JET(12) » 0 
JET(15) = 0 
JET(16) = 0 
700 CONTINUE 
GO TO 900 

^5000 CX = WVX2A*RTD 
CY = WVY2A*RTD 
CZ = WVZ2A*RTD 
EMX = 0 

IF(CX.GT.WXDB) EMX = -1 
IF(CX.LT.(-WXDB)) EMX = 1 
EAZ = 0 

IF(CY.GT.WYDB) EAZ = -1 
IF(CY.LT.(-WYDB)) EAZ = 1 


EAY = 0 

IF(CZ.GT.WZDB) EAY = 1 
IF(CZ.LT.(-WXDB)) EAY = -1 
IFX = 1 + EMX 

IFZ * 1 + EAZ 

IFY = 1 + EAY 

J = 1 + 9*IFX + 3*IFZ + IFY 

GO TO 0,(5001,5002,5003,5004,5005,5006,5007,5008,5009,5010, 
*5011,5012,5013,5014,5015,5016,5017,5018,5019,5020,5021,5022, 
*5023,5024,5025,5026,5027) 

5001 CONTINUE 
JET(2) = 1 
JET(6) = 1 
JET{15) = 1 
JET(21) = 1 
JET(17) = 1 
JET(23) * 1 
GO TO 900 

5002 CONTINUE 
JET(6) = 1 
JET(21) =1 
JET(2) = 1 
JET(8) = 1 
JET(17) = 1 
JET(23) = 1 
GO TO 900 

5003 CONTINUE 
JET(6) = 1 
JET(8) = 1 
JET(15) = 1 
JET(21) = 1 
JET(17) = 1 
JET{23) = 1 
GO TO 900 

5004 CONTINUE 
JET(2) = 1 
JET(23) = 1 
JET(15) = 1 
JET(21) = 1 
JET(6) = 1 
JET(12) = 1 
GO TO 900 

5005 CONTINUE 
JET(15) = 1 
JET(21) = 1 
JET(6) = 1 
JET(12) = 1 
GO TO 900 

5006 CONTINUE 
JET(8) = 1 
JET(17) = 1 
JET(15) * 1 
JET(21) = 1 


JET(6) = 1 
JET(12) = 1 
GO TO 900 

5007 CONTINUE 
JET(12) = 1 
JET(2) = 1 
JET(15) = 1 
JET(21) = 1 
JET(17) = 1 
JET(23) = 1 
GO TO 900 

5008 CONTINUE 
JET(12) = 1 
JET(15) = 1 
JET(2) = 1 
JET{8) = 1 
JET(17) = 1 
JET(23) = 1 
GO TO 900 

5009 CONTINUE 
0ET(12) = 1 
JET(8) = 1 
JET(15) = 1 
JET(21) = 1 
JET(17) = 1 
JET(23) = 1 
GO TO 900 

5010 CONTINUE 
JET(9) = 1 
JET(21) = 1 
JET(2) = 1 
JET(20) = 1 
GO TO 900 

5011 CONTINUE 
JET(9) = 1 
JET(21) = 1 
GO TO 900 

5012 CONTINUE 
JET(9) = 1 
JET(21) = 1 
JET(ll) = 1 
JET(17) = 1 
GO TO 900 

5013 CONTINUE 
JET( 2) = 1 
JET(20) * 1 
GO TO 900 

5014 CONTINUE 
GO TO 900 

5015 CONTINUE 
JET(17) * 1 
JET(ll) = 1 
GO TO 900 


5016 CONTINUE 
JET(3) = 1 
JET(15) = 1 
JET(2) = 1 
JET(20) * 1 
GO TO 900 

5017 CONTINUE 
JET(3) = 1 
JET(15) = 1 
GO TO 900 

5018 CONTINUE 
JET(3) = 1 
JET(15) = 1 
JET(ll) = 1 
JET(17) « 1 
GO TO 900 

5019 CONTINUE 
JET(9) = 1 
JET(5) = 1 
JET(14) = 1 
JET(20) * 1 
JET(18) = 1 
JET(24) = 1 
GO TO 900 

5020 CONTINUE 
JET(9) = 1 
JET(18) = 1 
JET(ll) = 1 
JET(14) = 1 
JET(5) = 1 
JET(20) = 1 
GO TO 900 

5021 CONTINUE 
JET(9) = 1 
JET(ll) = 1 
JET(14) = 1 
JET(20) = 1 
JET(18) = 1 
JET(24) = 1 
GO TO 900 

5022 CONTINUE 
JET(5) = 1 
JET(20) = 1 
JET(9) = 1 
JET(18) = 1 
JET(3) = 1 
JET(24) = 1 
GO TO 900 

5023 CONTINUE 
JET(9) = 1 
JET(18) = 1 
JET(3) = 1 
JET(24) = 1 
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GO TO 900 

5024 CONTINUE 
JET(ll) = 1 
JET(14) = 1 
JET{9) = 1 
JET(18) = 1 
JET(3) = 1 
JET(24) = 1 
GO TO 900 

5025 CONTINUE 
JET(3) = 1 
JET(5) = 1 
JET(14) = 1 
JET(20) = 1 
JET(18) = 1 
JET(24) = 1 
GO TO 900 

5026 CONTINUE 
JET(3) = 1 
JET(24) = 1 
■JET(ll) = 1 
JET(14) = 1 
JET( 5) = 1 
JET(20) = 1 
GO TO 900 

5027 CONTINUE 
JET(3) = 1 
JET(ll) = 1 
JET(14) = 1 
JET(20) = 1 
JET(18) = 1 
JET(24) = 1 

900 CONTINUE 
IF(ISDA.EQ.O) GO TO 902 
DO 901 1= 1,12 

II =ISIDEA(I) 

901 JET(II)=0 
GO TO 912 

902 IF(ISDB.EQ.O) GO TO 904 
DO 903 I = 1,12 
II *ISIDEB(I) 

903 JET(II)=0 
GO TO 912 

904 CONTINUE 

FAILED JETS AND EFFECTS 
DO 910 1=1,3 

IF(IJONS(I) .EO. 0) GO TO 908 
IF(IJETON(I) .EO. 0) GO TO 908 
II =IJETON(I) 

JET(II)=1 

908 IF(IJOFS(I) .EQ. 0) GO TO 910 



IF(IJETOF(I) .EQ. 0) GO TO 910 
II =IJETOF(I) 

JET(II)=0 
910 CONTINUE 

912 CONTINUE 
C 

C CALCULATE FORCES AND MOMENTS 
C 

SUMF =0.0 
SUMFX=0.0 

NCOUNT = NCOUNT + 1 
DO 920 1=1,24 

C IF JET IS JUST NOW TURNED ON COUNT AS A FIRING STARTED. 
IF(JET(I) .LE. JETN(I)) GO TO 101 
JETC(I)=JETC(I)+1 

C JETC(I)=COUNT OF NO. OF FIRINGS STARTED FOR JET(I) , 

C FOR THIS RUN. 

101 CONTINUE 

C IF THRUSTER STAYED OFF FROM LAST PASS, GO TO 920 
IF(JETN(I) .EO. 1)G0 TO 918 
RTIME(I)=0. 

IF(JET(I) .EO. 0) GO TO 920 
JTIME(I)=0 

C INCREMENT NO. OF FIRINGS THIS DATA PERIOD 
NFP=NFP+1 

C INCREMENT NO. OF FIRINGS THIS RUN. 

NFT=NFT+1 

JETN(I)=JET(I) 

GO TO 919 

918 JTIME(I)=JTIME(I)+1 
IF(JET(I) .EQ. 1) GO TO 919 

C CALCULATE ON TIME AND TOTAL ON TIME 
RTIME(I)=JTIME(I)*DT 
SUMRT( I ) =SUMRT{ I ) +RTIME( I ) 

JETN(I) = JET(I) 

GO TO 920 

C IF JET IS ON, ADD ITS THRUST LEVEL(FTR)TO FORCES & MOMENTS. 

919 CONTINUE 

IF(I .LE. 8)G0 TO 913 
SUMF =SUMF+1.0 
GO TO 914 

913 SUMFX=SUMFX+1 

914 FJX =FJX+FTRX*FM(I,1) 

FJY =FJY+FTR*FM(I,2) 

FJZ =FJZ+FTR*FM(I,3) 

MX =MX+FTR*FM(I,4) 

MY =MY+FTR*FM(I,5) 

MZ =MZ+FTR*FM(I,6) 

920 CONTINUE 

AFUEL=AFUEL+ ( DT/ 220 . ) * ( FTRX*SUMFX+FTR*SUMF) 

C THE NEXT TWO STATEMENTS ARE USED TO MAKE A FILE OF DATA 
C POINTS TO USE IN THE PLOT PROGRAM 

C WRITE(4,5100)NFT,AFUEL,R0,TIME 
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5100 F0RMAT(1X,I4,2(1X,F6.2),1X,E12.5) 

2000 CONTINUE 

IF DATA PERIOD NOT OVER, SKIP CALCULATION OF DURATION TIME. 
MPASS=MOD(JPASS,ISAMP) 

JPASS=NO. PROGRAM PASSES 
ISAMP=SAMPLING RATE ON HAND CONTROL INPUTS 
IF(MPASS .NE. 0) GO TO 927 
J=1 
L=1 

DO 921 1=1,24 
C NO ID SHOULD BE PRINTED IF IT IS EQUAL TO 99. 

ID(I)=99 

IF(RTIME(I) .EQ. 0.)G0 TO 922 
C PACK ID AND DURATION TIMES 
ID(J)=I 

STIME(J)=RTIME(I) 

J=J+1 

922 CONTINUE 

IF(SUMRT(I) .EQ. SUMRTL(I))GO TO 921 
C IF TOTAL ON TIME HAS CHANGED REPACK ID AND TOTAL TIME 
IDS^L)=I 

SSUMRT(L)=SUMRT(I) 

C SET TOTAL ON TIME LAST=TOTAL ON TIME 
SUMRTL(I)=SUMRT(I) 

L=L+1 

921 CONTINUE 

IF(J .NE. 1)G0 TO 924 
IF(NFP .EO. 0)G0 TO 926 
C WRITE(6,lOOO)NCOUNT,NFP,NFT,AFUEL,R0 
GO TO 925 
924 CONTINUE 
103 FORMATdH ,2X,I5) 

K=J-1 

C WRITE(7,103)K 

IF(K .GT. 10)G0 TO 811 
GO TO 925 

C GO TO (801, 802, 803, 804, 805, 806, 807,808, 809, 810), K 

801 WRITE(6,1001)NC0UNT,NFP,NFT,AFUEL,R0,ID(1),STIME(1) 

GO TO 925 

802 WRITE(6,1002)NCOUNT,NFP,NFT,AFUEL,RO,(ID(I),STIME(I),I=1,2) 
GO TO 925 

803 WRITE(6,1003)NC0UNT,NFP,NFT,AFUEL,R0,(ID(I),STIME(I),I=1,3) 
GO TO 925 

804 WRITE(6,1004)NC0UNT,NFP,NFT,AFUEL,R0,(ID(I),STIME(I),I=1,4) 
GO TO 925 

805 WRITE(6,1005)NC0UNT,NFP,NFT,AFUEL,R0,(ID(I),STIME(I),I=1,5) 
GO TO 925 

806 WRITE(6,1006)NC0UNT,NFP,NFT,AFUEL,R0,(ID(I),STIME(I),I=1,6) 
GO TO 925 

807 WRITE(6,1007)NC0UNT,NFP,NFT,AFUEL,R0,(ID(I),STIME(I),I=1,7) 
GO TO 925 

808 WRITE(6,1008)NC0UNT,NFP,NFT,AFUEL,R0,(ID(I),STIME(I),I=1,8) 
GO TO 925 
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809 WRITE(6,1009)NCOUNT,NFP,NFT,AFUEL,RO,(ID(I),STIME(I),I=1,9) 
GO TO 925 

810 WRITE(6,1010)NCOUNT,NFP,NFT,AFUEL,RO,(ID(I),STIME(I),I=1,10) 
GO TO 925 

NCOUNT IS THE CURRENT PROGRAM CYCLE IN THIS RUN. 

NFP IS NO. OF THURSTER FIRINGS PER DATA PERIOD 
NFT IS TOTAL NO. OF THRUSTER FIRINGS FOR THIS RUN 
JETS CAN GO ON OR OFF ONLY EVERY ISAMP PROGRAM PASS BECAUSE 
HAND CONTROLLER INPUTS ARE ACCEPTED THEN ONLY. IN MAIN 
ISAMP IS THE MAX. NO. OF PROGRAM CYCLES PER DATA PERIOD. 
JETN(I)= VALUE OF JET(I) AT LAST PROGRAM CYCLE. 

811 PAUSE 'K2LARGE' 

925 CONTINUE 
NFP=0 

926 CONTINUE 

IF(L .EQ. 1)G0 TO 927 
M=L-1 

C WRITE(7,103)M 

IF(M .GT. 10)G0 TO 928 
GO TO 927 

C GO TO (821, 822, 823, 824, 825, 826, 827, 828, 829,830), M 

928 PAUSE 'M2LARGE' 

GO TO 927 

821 WRITE(5,1021)NC0UNT,R0,IDS(1) ,SSUMRT(1) 

C WRITE(4,1040)SSUMRT{1),R0 

GO TO 927 

822 WRITE(5,1022)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,2) 

GO TO 927 

823 WRITE(5,1023)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,3) 

GO TO 927 

824 WRITE(5,1024)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,4) 

GO TO 927 

825 WRITE(5,1025)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,5) 

GO TO 927 

826 WRITE(5,1026)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,6) 

GO TO 927 

827 WRITE(5,1027)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,7) 

GO TO 927 

828 WRITE(5,1028)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,8) 

GO TO 927 

829 WRITE(5,1029)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,9) 

GO TO 927 

830 WRITE(5,1030)NC0UNT,R0,(IDS(I),SSUMRT(I),I=1,10) 

927 CONTINUE 
RETURN 

1050 DO 1041 1=1,24 
C WRITE(5, 1042)1, SUMRT(I) 

1042 F0RMAT(1H ,I3,2X,F6.2) 

1041 CONTINUE 
RETURN 

1000 F0RMAT(1H ,I5,1X,I2,1X,I4,2(1X,F6.2) ) 

1001 F0RMAT(1H ,I5,1X,I2,1X,I4,2(1X,F6.2) ,1(1X, 

* '(',1X,I2,'-',F5.2,')')) 


1002 F0RMAT(1H ,I5,1X,I2,1X,I4,2(1X,F6.2),2(1X, 

* ' ( ' ,1X, 12,' ,F5.2, ' ) ' ) ) 

1003 F0RMAT(iH *I5,ix,i2|lX,I4,2(lX,F6.2),3(lX, 

1004 F0RMAT(iH *,I5,lX,i2hx,I4,2(lX,F6.2),4(lX, 

1005 F0RMAT(1H ’,I5,ix,i24X,I4,2(lX,F6.2),5(lX, 

* ' ( ' ,1X,I2,'-' ,F5.2,' ) ' )) 

1006 F0RMAT(1H *,I5,lX,i2!lX,I4,2(lX,F6.2) ,6{1X, 

1007 F0RMAT(iH *,I5,ix,i24X,I4,2(lX,F6.2) ,7(1X, 

* ' ( ' ,1X,I2,' ,F5.2, ' ) ' ) ) 

1008 F0RMAT(1H *,I5,lX,i2!lX,I4,2(lX,F6.2) ,8(1X, 

* ' ( ' ,1X,I2,' ,F5.2, ' ) ' ) ) 

1009 F0RMAT(1H ’l5,ix,i2!lX,I4,2(lX,F6.2) ,9(1X, 

* ' ( ' ,1X,I2, ' ,F5.2, ' ) ' ) ) 

1010 F0RMAT(1H *,I5,ix,i2!lX,I4,2(lX,F6.2) ,10(1X, 

* •C,1X,I2,'-',F5.2,')')) 

1021 F0RMAT(1H ,I5,F6.2,1(1X, ' ( ' ,1X,I2,'-' ,F5.2,' ) ' ) ) 

1022 FORMATdH ,I5,F6.2,2(1X, ' ( ' ,1X,I2,'-' ,F5.2,' ) ' ) ) 

1023 FORMATdH ,I5,F6.2,3(1X,' ( ' ,1X,I2,'-' ,F5.2.' ) ' )) 

1024 F0RMAT(1H ,I5,F6.2,4(1X,' (' ,1X,I2,'-' ,F5.2,')')) 

1025 FORMATdH ,I5,F6.2,5(1X,' ( ' ,1X,I2,'-' ,F5.2,' ) ‘ ) ) 

1026 FORMATdH ,I5,F6.2,6(1X, ' { ' ,1X,I2,'-' ,F5.2,' ) ' ) ) 

1027 F0RMAT(1H ,I5,F6.2,7(1X, ' ( ' ,1X,I2,'-' ,F5.2, ' ) ' ) ) 

1028 FORMATdH ,I5,F6.2,8(1X,' ( ' ,1X,I2,'-' ,F5.2,' ) ' )) 

1029 FORMATdH ,I5,F6.2,9(1X,' { ' ,1X,I2,'-' ,F5.2,' )' )) 

1030 FORMATdH ,I5,F6.2,10(1X, ' ( ' ,1X,I2,' ,F5.2,' ) ' ) ) 
1040 F0RMAT(2(2X,F6.2)) 

END 



c 

C TBA DOCKING DYNAMICS SIMULATION 

C 

C AUTHOR: PATRICK TOBBE, JOHN GLAESE 

C 

C THIS SUBROUTINE IS THE DRIVER FOR THE CONTROL DYNAMICS 

C DOCKING DYNAMICS SUBROUTINE. THE INPUT CONSISTS OF THE 

C CONTROL SYSTEM JOY STICK DATA AND SENSOR OUTPUT VECTOR 

C KIN AT TIME T. THE SUBROUTINE PRODUCES THE RELATIVE POSITION 

C VECTOR AND TRANSFORMATION MATRIX BETWEEN THE CHASE AND 

C TARGET VEHICLES AT TIME + DT. 

C 

SUBROUTINE DYNAMIC 
C 

INCLUDE 'cmmn.f 
INCLUDE ’cmtnnl.f' 

INCLUDE 'cmmn2.f 
INCLUDE 'cmmncon.f' 

INCLUDE 'cmmnbb.f 

C 

INTEGER I 

C 

IF (NNN) 5,5,10 
5 CONTINUE 

CALL STARTT 
CALL INTFAC 
CALL FMTRAN 
CALL CNTRLC 
CALL CHASE 
CALL TARGET 
RETURN 

C 

C TOP OF INTEGRATION LOOP. 

C 

10 CONTINUE 

CALL FMTRAN 
CALL CNTRLC 
CALL CHASE 
CALL TARGET 
IF (T.EQ.0.0) THEN 
DO 1500 1=1, NST 

YD01(I)=YD1(I) 

YD02(I)=YD2(I) 

1500 CONTINUE 
ENDIF 

DO 1000 1=1, NST 

Y1(I)=Y1(I)+DT2*(3.*YD1(I)-YD01(I)) 

Y2(I)=Y2(I)+DT2*(3.*YD2(I)-YD02(I)) 

1000 CONTINUE 

C 

c 

c COMPUTE INTERFACE INFORMATION 



o o o o o o 


c 

C 

CALL INTFAC 


CALCULATE NEW OMEGA VECTORS 


CALL MVMUL(0MV1,AIV1I,HTV1) 
CALL MVMUL(OMV2,AIV2I,HTV2) 


NORMALIZE OUATERNIONS 


CALL NORMAL (OVIE) 
CALL NORMAL (0V2E) 

C 

RETURN 

END 


oooooooooooooooooooooooo o o o o ooooooooo 


THIS SUBROUTINE CALCULATES THE CONTACT FORCES AND MOMENTS 
ABOUT THE CENTERS OF MASS OF THE TARGET AND CHASE VEHICLES. 

THE INPUT IS THE KIN VECTOR FROM THE SENSOR 

THE OUTPUT CONSISTS OF THE VALUES OF IPOT, IFREEZE, IFLAG, AND 
NOUSE. 

SUBROUTINE FMTRAN 


SAVE 

INCLUDE 'cmmnl.f 
INCLUDE 'cmmn2.f 
INCLUDE 'cmmnbb.f 


INTEGER*2 THRESH(6) ,K0FF(8) 

INTEGER I 

REAL RS1V2L(3) ,FKM(3) ,MTKM(3) ,FSC(3) ,MTSC(3) ,AIN(6) 
REAL RS1V1L(3),WMAX,WMAXI 


DATA THRESH /lOO, 100, 200, 100, 100, 100/ 
DATA WMAX /3276S./ 

DATA FKM /lOOO. ,1000. ,1000./ 

DATA MTKM /lOOO. ,1000. ,1000./ 


KOFF IS THE OFFSET FROM FORCES COMING IN 
THRESH IS THRESHOLD LIMIT ON FORCES 
ORDER IS X,Y,Z, ROLL, YAW, PITCH 

THE MAXIMUM WORD IS 32768 (16 BITS) 

FKM AND MTKM ARE MAXIMUM OUTPUTS OF THE FORCE TABLE AS SCALED 
IN AMPS. ORDER IS X,Y,Z, ROLL, YAW, PITCH 


ALL FORCE AND MOMENT DATA RETURNING FROM THE SENSOR 
ARE IN SI OR SENSOR FRAME COORDINATES 


TlCSl IS THE TORQUE ACTING AT THE SENSOR IN SENSOR COORDINATES 
TlCGSl IS THE TORQUE ACTING ON THE TARGET VEHICLE CENTER OF MASS 
IN SENSOR COORDINATES. 

T2CGS1 IS THE TORQUE ACTING ON THE CHASE VEHICLE CENTER OF MASS 
IN SENSOR COORDINATES. 


ooo o oooo o o o o oooooooooooo 


FlCSl IS THE FORCE AT THE SENSOR IN SENSOR COORDINATES 


RSIVISI IS THE VECTOR FROM THE TARGET VEHICLE CENTER OF MASS 
TO THE SENSOR IN SENSOR COORDINATES 

RS1V2S1 IS THE VECTOR FROM THE CHASE VEHICLE CENTER OF MASS TO 
THE SENSOR IN SENSOR COORDINATES 


IF (NNN) 5,5,15 

COMPUTE SCALE FACTORS FOR FORCE TABLE INPUTS 
5 CONTINUE 

WMAXI=1./WMAX 

DO 3 1=1,3 
FSC(I)=FKM(I)*WMAXI 
MTSC(I)=MTKM(I)*WMAXI 
3 CONTINUE 

15 CONTINUE 
IP0T=KIN(9) 

IF(KIN(8) .EO. 0) GO TO 7 

NULL FORCE TABLE OFFSETS TO 0 
ICF0RCE=I2=KIN(8) 

DO 9 1=1,6 
9 KOFF(I)=KIN(I) 

IFLAG=1 

7 DO 8 1=1,6 
KIN(I)=KIN(I)-KOFF(I) 

IF(KIN(I) .GT. -THRESH(I) .AND. KIN(I) .LT. THRESH(I))KIN(I)=0 
IF(IFLAG .EO. 0)KIN(I)=0 
AIN(I)=FLOAT(KIN(I)) 

8 CONTINUE 

FSC IS SCALE FACTOR ON INPUTS ( X,Y,Z,ROLL, YAW, PITCH) 

F1CS1(1)=AIN(1)*FSC(1) 

F1CS1(2)=AIN(2)*FSC(2) 

F1CS1(3)=AIN(3)*FSC(3) 

T1CS1(1)=AIN(4)*MTSC(1) 

T1CS1(2)=AIN(6)*MTSC(3) 

T1CS1(3)=AIN(5)*MTSC(2) 

IFREEZ=KIN(7) 

N0USE=KIN(8) 


c 

C COMPUTE CONTACT TORQUES ON TARGET VEHICLE ABOUT ITS CENTER OF MASS 
C 

T1CGS1(1)=T1CS1(1)+RS1V1S1(2)*F1CS1(3)-RS1V1S1(3)*F1CS1(2) 

T1CGS1(2)=T1CS1(2)+RS1V1S1(3)*F1CS1(1)-RS1V1S1(1)*F1CS1(3) 

T1CGS1(3)=T1CS1{3)+RS1V1S1(1)*F1CS1(2)-RS1V1S1(2)*F1CS1(1) 

C 

C 

C TRANSFORM TARGET CONTACT TORQUES TO TARGET VEHICLE FRAME FROM 
C SENSOR FRAME 

C TRANSFORM TARGET CONTACT FORCES TO LOCAL VERTICAL FRAME FROM 
C SENSOR FRAME 

C 

CALL VMMUL(T1CGV1,T1CGS1,S1V1) 

CALL VMMUL(F1CL,F1CS1,S1L) 

C 

C 

C COMPUTE MOMENT ARM FROM CHASE VEHICLE CENTER OF MASS 

C TO THE SENSOR IN LOCAL VERTICAL COORDINATES. 

C 

CALL VMMUL (RS1V1L,RS1V1V1,V1L) 

DO 10 1=1,3 

RS1V2L(I)=R1L(I)+RS1V1L(I)-R2L(I) 

10 CONTINUE 
C 

C TRANSFORM MOMENT ARM TO SENSOR COORDINATES FROM TARGET VEHICLE 
C COORDINATES. 

C 

CALL MVMUL(RS1V2S1,S1L,RS1V2L) 

C 

C 

C ASSUME EQUAL AND QPPOSITE CQNTACT FORCES 

C 

DO 20 1=1,3 
F2CS1(I)=-F1CS1(I) 

20 CONTINUE 

C 

c 

C COMPUTE CONTACT TORQUES ABOUT CENTER OF MASS OF CHASE VEHICLE 

C IN SENSOR COORDINATES. 

C 

T2CGS1(1)=-T1CS1(1)+RS1V2S1(2)*F2CS1(3)-RS1V2S1(3)*F2CS1(2) 

T2CGS1 ( 2) =-TlCSl ( 2)+RSlV2Sl ( 3)*F2CS1 ( 1 ) -RS1V2S1 ( 1 )*F2CS1 ( 3) 

T2CGS1 ( 3) =-TlCSl ( 3)+RSlV2Sl ( 1)*F2CS1 ( 2) -RS1V2S1 ( 2)*F2CS1 ( 1 ) 

C 

C 

C TRANSFORM CHASE CONTACT TORQUES TO CHASE VEHICLE FRAME FROM 

C SENSOR FRAME 

C TRANSFORM CHASE CONTACT FORCES TO LOCAL VERTICAL FRAME FROM 

C SENSOR FRAME 

C 

CALL VMMUL(T2CGV2,T2CGS1,S1V2) 

CALL VMMUL(F2CL,F2CS1,S1L) 


RETURN 

END 


oooooooooo ooo ooooo o o ooooooooooooooooooo 


THIS SUBROUTINE CALCULATES THE NECESSARY TRANSFORMATION 
MATRICES USED IN THE DYNAMICS. 

IT OUTPUTS THE POSITION VECTOR FROM THE TARGET DOCKING 
PORT TO THE CHASE DOCKING PORT IN TARGET DOCKING PORT 
COORDINATES. 

IT OUTPUTS THE VELOCITY OF THE CHASE DOCKING PORT WITH 
RESPECT TO THE TARGET DOCKING PORT IN TARGET DOCKING 
PORT COORDINATES. 

IT OUTPUTS THE ANGULAR VELOCITY OF THE CHASE VEHICLE 
WITH RESPECT TO THE TARGET VEHICLE IN CHASE VEHICLE 
COORDINATES. 

IT ALSO OUTPUTS THE TRANSFORMATION MATRIX FROM THE CHASE 
DOCKING PORT TO THE TARGET DOCKING PORT COORDINATE FRAMES. 

SUBROUTINE INTFAC 


INCLUDE 'cmmn.f 
INCLUDE 'cmmnl.f 
INCLUDE 'cmmn2.f 
INCLUDE 'cmmnbb.f 


REAL RD1V1L(3),RD2V2L(3),RD2D1L(3),RD2D1LD(3),0MV1L(3) 
REAL 0MV2L(3) ,0MV1RL(3) ,0MV2V1L(3) ,WRXRD2(3) 

REAL WRLXR2(3),WRLXR1(3) 

INTEGER I,J,K 


CONSTRUCT COORDINATE TRANSFORMATIONS 


VIE 


CALL AFQ(V1E,QV1E) 


V2E 


CALL AFQ(V2E,QV2E) 


XLE IS THE TRANSFORMATION FROM THE INERTIAL E FRAME TO THE 
LOCAL VERTICAL L FRAME 


RNU2 IS THE CURRENT ORBIT ANGLE 


RNU2=RNU20+0M0*T 

C 

CALL EUL2(XLE,RNU2) 

CALL MMUL33(XLE,AE) 

C — — — — — 

C V1L=V1E*(XLE)T. V2L=V2E*(XLE)T. 

C 

C 

C CALCULATE TRANSFORMATIONS FROM THE LOCAL VERTICAL TO THE CHASE 
C AND TARGET VEHICLE FRAMES. 

C 

DO 900 1=1,3 
DO 900 J=l,3 
V1L(I,J)=0.0 
V2L(I,J)=0.0 
DO 900 K=l,3 

V1L(I,J)=V1L(I,J)+V1E(I,K)*XLE(J.K) 

V2L( I ,J)=V2L( I ,J)+V2E(I ,K)*XLE(J ,K) 

900 CONTINUE 

C 

r 

C COMPUTE TRANSFORMATION MATRIX FROM LOCAL VERTICAL FRAME 
C TO THE TARGET DOCKING PORT FRAME 
C 

CALL MMUL3(D1L,D1V1,V1L) 

C 

C 

C COMPUTE TRANSFORMATION MATRIX FROM LOCAL VERTICAL FRAME 
C TO THE CHASE DOCKING PORT FRAME 

C 

CALL MMUL3(D2L,D2V2,V2L) 

C 

C 

C COMPUTE TRANSFORMATION MATRIX FROM LOCAL VERTICAL FRAME 
C TO THE SENSOR FRAME 

C 

CALL MMUL3(S1L,S1V1,V1L) 

C 

C 

C CALCULATE TRANSFORMATION FROM CHASE VEHICLE TO TARGET VEHICLE 
C DOCKING PORT COORDINATES. 

C 

DO 1540 1=1,3 
DO 1540 J=l,3 
D1D2(I,J)=0.0 
DO 1540 K=l,3 

D1D2( I ,J)=D1D2( I ,J)+D1L( I ,K)*D2L( J,K) 

1540 CONTINUE 

C 

c 

C CALCULATE TRANSFORMATION FROM CHASE VEHICLE TO SENSOR COORDINATES 
C 

DO 1550 1=1,3 


DO 1550 0=1,3 
S1V2(I,J)=0. 

DO 1550 K=l,3 

S1V2(I,J)=S1V2(I,J)+S1L(I,K)*V2L(J,K) 

1550 CONTINUE 

C 

C 

C CALCULATE POSITION AND VELOCITY OF CHASE VEHICLE DOCKING PORT 
C WITH RESPECT TO THE TARGET VEHICLE DOCKING PORT 

C 

C 

CALL VMMUL (RDIVIL.RDIVIVI.VIL) 

CALL VMMUL (RD2V2L,RD2V2V2,V2L) 

C 

C RD2D1L IS THE POSITION OF THE CHASE DOCKING PORT WITH RESPECT 

C TO THE TARGET DOCKING PORT IN LOCAL VERTICAL COORDINATES 

C 

DO 10 1=1,3 

RD2D1L(I)=R2L(I)+RD2V2L(I)-R1L(I)-RD1V1L(I) 

10 CONTINUE 

C 

C OMVIL AND 0MV2L ARE THE ANGULAR VELOCITIES OF THE TARGET AND CHASE 

C VEHICLES, RESPECTIVELY, IN LOCAL VERTICAL COORDINATES 

C 

CALL VMMUL (OMVIL, OMVl ,V1L) 

CALL VMMUL (0MV2L,0MV2,V2L) 

C 

C 0MV2V1L IS THE ANGULAR VELOCITY OF THE CHASE VEHICLE WITH 

C RESPECT TO THE TARGET VEHICLE IN LOCAL VERTICAL COORDINATES 

C 

DO 20 1=1,3 
0MV1RL(I)=0MV1L(I) 

0MV2V1L(I)=0MV2L(I)-0MV1L(I) 

20 CONTINUE 

C 

C OMVIRL IS THE ANGULAR VELOCITY OF THE TARGET 
C VEHICLE WITH RESPECT TO THE LOCAL VERTICAL 
C ORBITAL RATE, IN LOCAL VERTICAL COORDINATES. 

C 

0MV1RL(2)=0MV1RL(2)-0M0 

C 

CALL CROSS (WRXRD2,0MV2V1L,RD2V2L) 

CALL CROSS (WRLXRl, OMVIRL, RIL) 

CALL CROSS (WRLXR2, OMVIRL, R2L) 

C 

DO 30 1=1,3 

RD2D1LD( I )=R2LD( I ) -WRLXR2( I)+WRXRD2( I )-RlLD( I )+WRLXRl( I ) 

30 CONTINUE 

C 

C 

C TRANSFORM ALL RELATIVE DOCKING PORT DATA FROM THE L FRAME TO 
C THE D1 FRAME. 

C 


c 

CALL MVMUL(RD2D1D1,D1L,RD2D1L) 
CALL MVMUL(RD2D1D1D,D1L,RD2D1LD) 
CALL MVMUL(0MV2V1V2,V2L,0MV2V1L) 

C 

RETURN 

END 


ooo ooo ooooo 


XFORMS 


SUBROUTINE TO GENERATE EULER ROTATION MATRIX ABOUT Y AXIS 


SUBROUTINE EUL2(A,EA) 
REAL A(3,3),EA 
A(2,2)=1.0 
A(2,3)=0. 

A(2,l)=0. 

A(l,2)=0. 

A(3,2)=0. 

A(1,1)=C0S(EA) 

A(3,3)=A(1,1) 

A(3,1)=SIN(EA) 

A(l,3)=-A(3,l) 

RETURN 

END 


SUBROUTINE TO GENERATE EULER ROTATION MATRIX ABOUT Z AXIS 


SUBROUTINE EUL3(A,EA) 
REAL A(3,3),EA 
A(3,3)=1.0 
A(3,l)=0. 

A(3,2)=0. 

A(l,3)=0. 

A(2,3)=0. 

A(1,1)=C0S(EA) 

A(2,2)=A(1,1) 

A(1,2)=SIN(EA) 

A(2,l)=-A(l,2) 

RETURN 

END 


MATRIX PRODUCT OF 3X3 MATRICES WITH RESULT IN LEFT FACTOR 


SUBROUTINE MMUL33(A,B) 

REAL A(3,3),B(3,3),C(3,3) 

INTEGER I,J,K 

DO 10 1=1,3 

DO 10 J=l,3 

C(I,J)=0. 

DO 10 K=l,3 

C(I,J)=C(I,J)+A(I,K)*B(K,J) 
10 CONTINUE 

DO 20 1=1,3 
DO 20 J=l,3 
A(I,J)=C(I,J) 

20 CONTINUE 

RETURN 
END 


C 

C MATRIX PRODUCT OF 3X3 MATRICES WITH RESULT IN LEFT FACTOR 

C 

SUBROUTINE MMUL3(C,A,B) 

REAL A(3,3),B(3,3),C(3,3) 

INTEGER I,J,K 
DO 10 1=1,3 
DO 10 J=l,3 
C(I,J)=0. 

DO 10 K=l,3 

C(I,J)=C(I,J)+A(I,K)*B(K,J) 

10 CONTINUE 
RETURN 
END 

C 

C MATRIX VECTOR PRODUCT 

C 

SUBROUTINE MVMUL(C,A,B) 

REAL C(3),A(3,3),B(3) 

INTEGER 1,0 
DO 10 1=1,3 
C(I)=0. 

DO 10 0=1,3 
C(I)=C(I)+A(I,0)*B(0) 

10 CONTINUE 

RETURN 
END 

C 

C VECTOR MATRIX PRODUCT 

C 

SUBROUTINE VMMUL(C,A,B) 

REAL C(3),A(3),B(3,3) 

INTEGER 1,0 
DO 10 1=1,3 
C(I)=0. 

DO 10 0=1,3 
C(I)=C(I)+A(0)*B(0,I) 

10 CONTINUE 

RETURN 
END 

C 

C SUBROUTINE TO COMPUTE DIRECTION COSINE MATRIX FROM OUATERNION 

C 

SUBROUTINE AF0(A,0) 

REAL A(3,3) ,0(4) ,01S0,02S0,Q3S0,04SQ 

REAL 0112,0113,0114,0123,0124,0134 

01S0=0(1)**2 

02S0=0(2)**2 

03S0=0(3)**2 

04S0=0(4)**2 

0112=2. 0*0(1)*Q(2) 

0113=2.0*0(1)*0(3) 

0H4=2.0*0(1)*0(4) 


o o o 


0123=2. 0*0(2)*0(3) 

0124=2. 0*0(2)*0(4) 

0134=2. 0*0(3)*0(4) 

A( 1 ,1 ) =01S0-O2SO-03SO+04S0 

A( 2 ,2) =-01SO+02SO-03SO+Q4SO 

A(3,3)=-01SO-02SO+03SO+04SO 

A(l,2)=0112+0134 

A(2,l)=0112-0134 

A(3.1)=0113+0124 

A{1.3)=0113-0124 

A(2,3)=0123+0114 

A(3.2)=0123-0114 

RETURN 

END 


SUBROUTINE TO COMPUTE OUATERNIONS FROM DIRECTION COSINES 


SUBROUTINE 0FA(0,A) 

REAL S{4,4) ,SP(4,4) ,A{3,3) ,0(4) ,TR,SPMAX,SPJ ,SGN 
INTEGER I,J 
nn in t=i 

DO 10 3=1 ^3 
S(I,J)=A{I,J) 

10 CONTINUE 

S(1,4)=A(2.3) 

S(2,4)=A(3,1) 

S(3,4)=A(1,2) 

TR=A(1,1)+A(2,2)+A(3,3) 

S(4,4)=TR 

S(4,l)=-A(3,2) 

S(4,2)=-A(l,3) 

S(4,3)=-A(2,l) 

DO 20 1=1,4 
DO 20 0=1,4 
SP(I,J)=S(I,J)+S{J,I) 

20 CONTINUE 

TR=1.0-TR 
DO 30 1=1,4 
SP(I,I)=SP(I,I)+TR 
30 CONTINUE 

1 = 1 

SPMAX=ABS(SP(1,1)) 

DO 40 0=2,4 
SP0=ABS(SP(0,0)) 

IF (SPO.LT.SPMAX) GOTO 35 
1=0 

SPMAX=ABS(SP(0,0)) 

35 CONTINUE 

40 CONTINUE 

SPMAX=SORT(SPMAX) 

DO 50 0=1,4 
0(0)=.5*SP(I,0)/SPMAX 
50 CONTINUE 


SGN=1.0 

IF (0(4).LT.0.) SGN=-1.0 
DO 60 1=1,4 
0(I)=0(I)*SGN 
60 CONTINUE 

RETURN 
END 

C 

C SUBROUTINE TO CALCULATE THE INVERSE OF 3X3 MATRIX 

C 

SUBROUTINE AINV (A,B) 

REAL A(3,3) ,B(3,3) ,C0FA(3,3) ,DETA 
INTEGER I,J 

C0FA(1,1)=A(2,2)*A(3,3)-A(2,3)*A(3,2) 

C0FA(1,2)=A(2,3)*A(3,1)-A(2,1)*A(3,3) 

C0FA(1,3)=A(2,1)*A(3,2)-A(2.2)*A(3,1) 

C0FA(2,1)=A(1,3)*A(3,2)-A(1,2)*A(3.3) 

C0FA(2,2)=A(1,1)*A(3,3)-A(1,3)*A(3,1) 

C0FA(2,3)=A(1,2)*A(3,1)-A(1,1)*A(3,2) 

C0FA(3,1)=A(1,2)*A(2,3)-A(1,3)*A(2,2) 

C0FA(3,2)=A(1,3)*A(2,1)-A(1,1)*A(2,3) 

C0FA(3,3)=A(1,1)*A(2,2)-A(1,2)*A(2,1) 

DETA=A(1,1)*C0FA(1,1)+A(1,2)*C0FA(1,2) 

$+A(l,3)*C0FA(l,3) 

DO 126 1=1,3 
DO 126 J=l,3 
B(I,J)=COFA(J,I)/DETA 
126 CONTINUE 
RETURN 
END 

C 

C SUBROUTINE TO NORMALIZE OUATERNIONS 


SUBROUTINE NORMAL (0) 

REAL 0(4), ON 
INTEGER I 
ON = 0.0 
DO 10 1=1,4 
ON = ON + 0(I)**2 
10 CONTINUE 

0N=1.5-.5*0N 
DO 20 1=1,4 
0(I)=0(I)*0N 
20 CONTINUE 

RETURN 
END 

C 

C SUBROUTINE TO CALCULATE OUATERNION FROM EULER ANGLE SEQUENCE 

C 1,2,3 

C 

SUBROUTINE OINT(AA1,AA2,AA3,0) 

REAL AA1,AA2,AA3,0(4),DTR 

REAL AAIR ,AA2R ,AA3R ,CC1 ,CC2 ,CC3 ,SS1 ,SS2 , SS3 


o o o 


DTR=. 

.0174532 

AAIR 

=AA1*DTR*.5 

AA2R 

=AA2*DTR*.5 

AA3R 

=AA3*DTR*.5 

CCl 

=C0S(AA1R) 

SSI 

=SIN(AA1R) 

CC2 

=C0S(AA2R) 

SS2 

=SIN(AA2R) 

CC3 

=C0S(AA3R) 

SS3 

=SIN(AA3R) 

0(1) 

=CC2*CC3*SS1+SS2*SS3*CC1 

0(2) 

=SS2*CC3*CC1-CC2*SS3*SS1 

0(3) 

=CC2*SS3*CC1+SS2*CC3*SS1 

0(4) 

=CC2*CC3*CC1-SS2*SS3*SS1 

RETURN 

END 



SUBROUTINE TO CALCULATE CROSS PRODUCT C = A X B 


SUBROUTINE CROSS (C,A,B) 

REAL C(3),A(3),B(3) 

C(1)=A(2)*B(3)-A(3)*B(2) 

C(2)=A(3)*B(1)-A(1)*B(3) 

C(3)=A(1)*B(2)-A(2)*B(1) 

RETURN 

END 


OOOOOOOOOO DOOOOO o o o o o ooooooooo 


INITIALIZATION SECTION 

THIS SUBROUTINE DEFINES THE MASS PROPERTIES AND GEOMETRY 
OF THE CHASE AND TARGET VEHICLES. 

IT ALSO DEFINES THE INITIAL VEHICLE CONDITIONS, ORBITAL 
PARAMETERS, AND THE CONTROL SYSTEM VARIABLES. 


SUBROUTINE STARTT 


INCLUDE 'cmmn.f 
INCLUDE 'ciminl.f' 
INCLUDE 'cmmnZ.f 
INCLUDE 'cmmncon.'f 
INCLUDE 'cmmnbb.f 


REAL 0DUM(4) ,R0RBK,GMEK,ALTK,REAK,RLA2K,RLA3K 
REAL PI,TEM2(3,3),RFD,XKFM 
INTEGER I,J 


PI=3. 1415926 
RFD=PI/180. 


DIMENSION OF THE STATE VECTOR 
NST=13 


THE ORIGIN OF THE VEHICLE FIXED FRAME, V, IS AT THE CENTER OF MASS 


TARGET VEHICLE MASS 
MASS1=118.11 


THE VEHICLE MOMENT OF INERTIA MATRIX IS IN THE VEHICLE FIXED FRAME 
AND IS ABOUT THE VEHICLE CENTER OF MASS. 

AIV IS THE INERTIA MATRIX 

AIVI IS INVERSE THE OF INERTIA MATRIX 


TARGET VEHICLE MOMENT OF INERTIA MATRIX BY ROWS 
DO 10 1=1,3 
DO 10 J=l,3 

AIV1(I,J)=0.0 

10 CONTINUE 

AIV1(1,1)=723.41 
AIV1(2,2)=2290.8 
AIV1(3,3)=2290.8 
CALL AINV (AIVI.AIVII) 

C 


THE INITIAL VEHICLE POSITION AND VELOCITY VECTORS ARE WITH RESPECT 
TO THE LOCAL VERTICAL, L, FRAME AND EXPRESSED IN THE LOCAL VERTICAL 
FRAME COORDINATES. 


C 

C 

C 

C 

C 

C 

C 


20 

C- 

C 

C 

C 

C 

C- 

C 

C 


5 

C 

C 

C 

C 

C 


C 

C 

C 

C 


25 


C 

C 

C 


30 

C- 


INITIAL TARGET VEHICLE LINEAR POSITION AND RATE 
DO 20 1=1,3 
R1L(I)=0.0 
R1LD(I)=0.0 
CONTINUE 


THE VEHICLE ANGULAR RATES ARE WITH RESPECT TO THE INERTIAL, E, 
FRAME AND EXPRESSED IN THE VEHICLE FRAMES. 


INITIAL TARGET VEHICLE 

IN DEGREES / SECOND 

0MV1(1)=0.0 

0MV1(2)=0.0 

0MV1(3)=0.0 

DO 5 1=1,3 

/ T \ f j\ *Dcn 
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CONTINUE 


INERTIAL ANGULAR RATES 


EULER ANGLES, IN DEGREES, FOR TYPE 1,2,3 SYSTEM TO DEFINE 
TRANSFORMATION FROM LOCAL VERTICAL TO TARGET FRAME 
DEFINES TRANSFORMATION VIL 


CALL QINT (0.,0.,0.,ODUM) 
CALL AFO (VIL, ODUM) 

CHASE VEHICLE MASS 
MASS2=326.23 


CHASE VEHICLE MOMENT OF INERTIA MATRIX BY ROWS 
DO 25 1=1,3 
DO 25 J=l,3 

AIV2(I,J)=0.0 

CONTINUE 

AIV2(1,1)=6221. 

AIV2(2,2)=3216. 

AIV2(3,3)=339A. 

CALL AINV (AIV2,AIV2I) 


INITIAL CHASE VEHICLE LINEAR POSITION AND RATE 
WITH RESPECT TO LOCAL VERTICAL IN LOCAL VERTICAL COORD 
DO 30 1=1,3 
R2L(I)=n.O 
R2LD(I)=0.0 
CONTINUE 


oooooooooooooooooooooooooooooooooo ooooo 


C INITIAL CHASE VEHICLE INERTIAL ANGULAR RATES 

C IN DEGREES / SECOND 

0MV2(1)=0.0 
0MV2(2)=0.0 
0MV2(3)=0.0 
DO 15 1=1,3 

0MV2(I)=0MV2(I)*RFD 
15 CONTINUE 


EULER ANGLES, IN DEGREES, FOR TYPE 1,2,3 SYSTEM TO DEFINE 
TRANSFORMATION FROM LOCAL VERTICAL TO CHASE FRAME 
DEFINES TRANSFORMATION V2L 


CALL OINT (180., 0., 180., ODUM) 
CALL AFQ (V2L,0DUM) 


INPUT DOCKING/BERTHING PORT DEFINITION DATA 


THE DOCKING PORT LOCATION VECTOR IS FROM THE VEHICLE CENTER OF MASS 
TO THE ORIGIN OF THE DOCKING PORT FRAME. IT IS EXPRESSED IN 
THE COORDINATES OF THE VEHICLE FIXED FRAME. 


oi 
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D1 IS THE DOCKING PORT COORDINATE FRAME ON THE TARGET VEHICLE 

D2 IS THE CHASE VEHICLE DOCKING PORT COORDINATE FRAME 

SlVl IS THE TRANSFORMATION MATRIX FROM TARGET VEHICLE COORDINATES 
TO SENSOR COORDINATES. 


DlVl IS THE TRANSFORMATION MATRIX FROM TARGET VEHICLE COORDINATES 
TO DOCKING PORT COORDINATES 


D2V2 IS THE TRANSFORMATION MATRIX FROM CHASE VEHICLE COORDINATES TO 
DOCKING PORT COORDINATES OF THE CHASE VEHICLE 

RSIVIVI IS THE POSITION VECTOR OF THE SENSOR IN TARGET VEHICLE COORDINATES 
WITH RESPECT TO THE TARGET CENTER OF MASS. 


RD2V2V2 IS THE POSITION VECTOR OF THE CHASE DOCKING PORT IN CHASE 
VEHICLE COORDINATES WITH RESPECT TO THE CHASE CENTER OF MASS. 

RDIVIVI IS THE POSITION VECTOR OF THE DOCKING PORT IN TARGET 
VEHICLE COORDINATES WITH RESPECT TO THE TARGET CENTER OF MASS. 


SlVl TRANSFORMATION BY ROWS 
DO 45 1=1,3 
DO 45 J=l,3 

S1V1(I,J)=0.0 


45 


CONTINUE 


ooooo o o ooo ooo 


S1V1(1,3)=1. 

S1V1(2,2)=1. 

S1V1(3.1)=-1. 

C 

C D2V2 TRANSFORMATION BY ROWS 

DO 40 1=1,3 
DO 40 J=l,3 

D2V2(I,J)=0.0 

40 CONTINUE 

D2V2(1,1)=1. 

D2V2(2,2)=1. 

D2V2(3,3)=1. 

C 

C DlVl TRANSFORMATION BY ROWS 

DO 50 1=1,3 
DO 50 J=l,3 

D1V1(I,J)=0.0 

50 CONTINUE 

D1V1(1,1)=-1. 

D1V1(2,2)=1. 

D1V1(3,3)=-1. 

INPUT SENSOR LOCATION VECTOR 
ON TARGET VEHICLE 
RS1V1V1(1)=1. 91667 
RS1V1V1(2)=0.0 
RS1V1V1(3)=0.0 

INPUT DOCKING PORT LOCATION VECTOR 
ON CHASE VEHICLE 
RD2V2V2(1)=3. 78125 
RD2V2V2(2)=0.0 
RD2V2V2(3)=0.0 

INPUT DOCKING PORT LOCATION VECTOR ON TARGET VEHICLE 
RD1V1V1(1)=4.0 
RD1V1V1{2)=0.0 
RD1V1V1(3)=0.0 


TRANSFORM SENSOR POSITION VECTOR FROM TARGET VEHICLE COORDINATES 
TO SENSOR COORDINATES. 

CALL MVMUL(RS1V1S1,S1V1,RS1V1V1) 

C 

C INITIAL ORBIT ANGLE NU20 IN DEGREES 
RNU20=0. 

RNU20=RNU20*RFD 

C 

C NOMINAL ORBIT ALTITUDE IN NAUTICAL MILES 
ALTK=260. 

C 

C ASCENDING NODE ANGLE IN DEGREES 


RLA2K=0. 

RLA2K = RLA2K * RFD 

C 

C INCLINATION ANGLE IN DEGREES 
RLA3K=0. 

RLA3K = RLA3K * RFD 

C 

XKFM=12. 0*6076. 0/39. 37E3 

C 

ALTK=ALTK*XKFM ! ALTITUDE IN KM 

C 

GMEK=3.9886E5 !KM**3-S**-2 

gmek=0.0 

C 

C 

C CALCULATE THE RADIUS AND FREQUENCY OF THE NOMINAL CIRCULAR ORBIT 
C REAK IS THE RADIUS OF THE EARTH IN KILOMETERS. 

C 

C 

REAK=6371.2 

RORBK=REAK+ALTK 

0M0SQ=GMEK/R0RBK**3 

C 

C 

C OMO IS THE FREQUENCY OF THE NOMINAL CIRCULAR ORBIT 
C 

OMO=SQRT(OMOSQ) 

C 

C 

C CALCULATE THE INITIAL ANGULAR MOMENTUM VECTOR FOR EACH VEHCILE. 

C THESE ARE EXPRESSED IN THE VEHICLE FIXED FRAMES. 

C 

C 

CALL MVMUL(HTVl.AIVl.OMVl) 

CALL MVMUL(HTV2,AIV2,0MV2) 

C 

DO 140 1=1, NST 
YD1(I)=0.0 
Y02(I)=0.0 
140 CONTINUE 

C 

c 

C CALCULATE THE INITIAL TRANSFORMATIONS FROM THE INERTIAL 
C FRAME TO THE VEHICLE FIXED FRAMES. 

C 

C 

CALL EUL2(TEM2,RLA2K) 

CALL EUL3(AE.RLA3K) 

CALL MMUL33(AE,TEM2) 

CALL EUL2(XLE,RNU20) 

CALL MMUL33(XLE,AE) 

CALL MMUL3(V1E,V1L,XLE) 

CALL MMUL3(V2E,V2L,XLE) 


CALL QFA(QV1E,V1E) 

CALL 0FA(0V2E,V2E) 

C 

C 

C INPUT INITIAL DATA FOR OMV CONTROL SYSTEM 
C 

C 

RTD=57. 29578 
DTR=. 0174532 
IS=0 
C 

NDC=TD/OT 
NDCI=NDC+1 
DO 180 I=1,NDCI 
DO 180 J=l,6 

DELAY(I,J)=0.0 

180 CONTINUE 
C 

C CURRENT NUMBER OF A-D SIGNALS IN THE CONTROL VECTOR KOAT IS 7 

C 

NAD=7 
AFUEL=0.0 
DO 190 1=1,3 

IJ0NS(I)=0.0 

IJ0FS(I)=0.0 

190 CONTINUE 

IAUT0P=0 
MPASS=0 
ISTBSL=0 
N0ISE1=.05 
N0ISE2=.05 
NOISO=NOISE1**2 
ISAMP=1 
C 

C SET SCALE FACTORS FOR A-D 

C 

SCX=2. 

SCY=2. 

SCZ=2. 

DO 220 1=1,7 

SSIN(I)=1. 

220 CONTINUE 

SSIN(4)=10./6.7 

SSIN(5)=10./3.8 

SSIN(6)=10./7.9 

YIN=l./2047. 

C 

C SET INITIAL ERROR ANGLES TO ZERO 

C 

CALL 0INT(0.,0.,0.,00UM) 

P31=0DUM(1) 

P32=00UM(2) 

P33=0DUM(3) 




c 

c 

C PHYSICAL SYSTEM (PLANT) SUBROUTINE FOR TARGET VEHICLE 

C 

C THE TARGET VEHICLE IS DENOTED BY THE NUMBER 1. 

C THIS SUBROUTINE CALCULATES THE TIME DERIVATIVE OF THE 

C TARGET VEHICLE STATE VECTOR. IT USES NEWTON-EULER AND 

C HILL'S EQUATIONS. 

C 

C THE INPUT TORQUES ARE FROM THE VEHICLE CONTACT AND GRAVITY 

C GRADIENT. 

C 

C THE INPUT FORCES ARE FROM THE VEHICLE CONTACT. 

C 

C 

SUBROUTINE TARGET 

C 

INCLUDE 'cmmn.f 
INCLUDE 'cmmnl.f 

C 

REAL VTEM(3) 

INTEGER I,J 

C 

C EXTRACT THE PHYSICAL PARAMETERS OUT OF THE STATE VECTOR Y. 

C 

C ASSUME THAT THE STATE VECTOR IS CONSTRUCTED AS FOLLOWS: 

C Y=(HTV1,OV1E,R1LO,R1L) 

C 

C HTVl IS THE ANGULAR MOMENTUM VECTOR OF THE TARGET VEHICLE. 

C QVIE IS THE QUATERNION FOR THE TRANSFORMATION FROM THE 

C INERTIAL. E. FRAME TO THE TARGET VEHICLE FIXED FRAME. 

C RILD IS THE VELOCITY OF THE TARGET VEHICLE CENTER OF MASS 

C WITH RESPECT TO THE L FRAME AND IN L FRAME COORDINATES. 

C RIL IS THE POSITON OF THE TARGET VEHICLE CENTER OF MASS 

C WITH RESPECT TO THE L FRAME AND IN L FRAME COORDINATES. 

C 

C 

DO 100 1=1, NST 
YD01(I)=YD1(I) 

100 CONTINUE 

C 

C COMPUTE THE TOTAL GRAVITY GRADIENT TORQUES 

C 

DO 1500 1=1,3 
VTEM(I)=0.0 
DO 1500 J=l,3 

VTEM(I)=VTEM(I)+AIV1(I,J)*V1L(J,3) 

1500 CONTINUE 

T1G(1)=3.0*OMOSO*(V1L(2,3)*VTEM(3)-V1L(3,3)*VTEM(2)) 

T1G(2)=3.0*0M0S0*(V1L(3,3)*VTEM(1)-V1L(1,3)*VTEM(3)) 


oooooo ooooo oooooo 


T1G(3)=3.0*0M0SQ*(V1L(1,3)*VTEM(2)-V1L(2,3)*VTEM(D) 


CALCULATE THE DERIVATIVE OF THE ANGULAR MOMENTUM VECTOR 
USING NEWTON-EULER EQUATION. 


YD1(1)=T1G(1)+T1CGV1(1)-0MV1(2)*HTV1(3)+0MV1(3)*HTV1(2) 

YD1(2)=T1G(2)+T1CGV1(2)-0MV1(3)*HTV1(1)+0MV1(1)*HTV1(3) 

YD1(3)=T1G(3)+T1CGV1(3)-0MV1(1)*HTV1(2)+0MV1(2)*HTV1(1) 


CALCULATE THE DERIVATIVE OF THE QUATERNION 


YD1{4)=.5*(0MV1(3)*QV1E(2) 

$ -OMV1(2)*OV1E(3)+OMV1(1)*QV1E(4)) 

YD1(5)=.5*(-0MV1(3)*0V1E(1) 

$ +0MV1(1)*QV1E(3)+0MV1(2)*QV1E(4)) 

YD1{6)=.5*{0MV1(2)*QV1E(1) 

$ -0MV1(1)*0V1E{2)+0MV1(3)*QV1E(4)) 

YD1(7)=-.5*{0MV1(1)*QV1E(1) 

$ +0MV1(2)*QV1E(2)+0MV1(3)*QV1E(3)) 


CALCULATE THE ACCELERATION OF THE VEHICLE CENTER OF MASS 
WITH RESPECT TO THE L FRAME IN THE L FRAME COORDINATES. 


YD1(8)=F1CL(1)/MASS1-2.*0M0*R1LD(3) 

YD1(9)=F1CL(2)/MASS1-0M0SQ*R1L(2) 

YD1(10)=F1CL(3)/MASS1+3.*0M0S0*R1L(3)+2.*0M0*R1LD(1) 

YD1(H)=Y1(8) 

YD1(12)=Y1(9) 

YD1(13)=Y1(10) 


RETURN 

END 


